diff --git a/.catkin_tools/VERSION b/.catkin_tools/VERSION
index 7deb86fee42803fdce673cb574b8186909c17806..899f24fc754a14b89617093824d258aaa3aa2943 100644
--- a/.catkin_tools/VERSION
+++ b/.catkin_tools/VERSION
@@ -1 +1 @@
-0.7.1
\ No newline at end of file
+0.9.0
\ No newline at end of file
diff --git a/.catkin_tools/profiles/default/config.yaml b/.catkin_tools/profiles/default/config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0967ef424bce6791893e9a57bb952f80fd536e93
--- /dev/null
+++ b/.catkin_tools/profiles/default/config.yaml
@@ -0,0 +1 @@
+{}
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 11f581c86b4fd6b6f1fc110e7a6e41d3a9116def..91f6f8aa6bff048bfe96c8122f6d32ba43bbfadd 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,2 +1,4 @@
 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 6278
+/home/matteo/reachability/devel/./cmake.lock 7314
+/home/matteo/reachability/devel/lib/libmoveit_grasps.so 66
+/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 66
diff --git a/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt b/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..34be7681767050b3986660d3a998b4f2bb577d2b
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_grasps/devel_manifest.txt
@@ -0,0 +1,12 @@
+moveit_grasps
+/home/matteo/reachability/devel/.private/moveit_grasps/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/moveit_grasps/share/moveit_grasps/cmake/moveit_graspsConfig.cmake /home/matteo/reachability/devel/share/moveit_grasps/cmake/moveit_graspsConfig.cmake
+/home/matteo/reachability/devel/.private/moveit_grasps/share/moveit_grasps/cmake/moveit_graspsConfig-version.cmake /home/matteo/reachability/devel/share/moveit_grasps/cmake/moveit_graspsConfig-version.cmake
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_pipeline_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_pipeline_demo
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_filter_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_filter_demo
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_poses_visualizer_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_poses_visualizer_demo
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_grasp_generator_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_grasp_generator_demo
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/moveit_grasps/moveit_grasps_suction_grasp_pipeline_demo /home/matteo/reachability/devel/lib/moveit_grasps/moveit_grasps_suction_grasp_pipeline_demo
+/home/matteo/reachability/devel/.private/moveit_grasps/lib/pkgconfig/moveit_grasps.pc /home/matteo/reachability/devel/lib/pkgconfig/moveit_grasps.pc
diff --git a/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml b/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2569b4a68c3450fb9b5276dec357aa71816699f3
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/moveit_grasps/package.xml
@@ -0,0 +1,50 @@
+<package>
+  <name>moveit_grasps</name>
+  <version>2.0.0</version>
+  <description>
+    A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline.
+    Does not consider friction cones or other dynamics.
+  </description>
+  <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer>
+  <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url>
+  <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url>
+  <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url>
+
+  <author email="davetcoleman@gmail.com">Dave Coleman</author>
+  <author email="mcevoy.andy@gmail.com">Andy McEvoy</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>eigen_conversions</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf_conversions</build_depend>
+  <build_depend>moveit_ros_planning</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_visual_tools</build_depend>
+  <build_depend>rosparam_shortcuts</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>moveit_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>moveit_visual_tools</run_depend>
+  <run_depend>rosparam_shortcuts</run_depend>
+
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+  <test_depend>panda_moveit_config</test_depend>
+
+</package>
diff --git a/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..79543793de7e2029e1c65c2c5b9f5ac83e981eb6
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt
@@ -0,0 +1,14 @@
+mtc
+/home/matteo/reachability/devel/.private/mtc/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-paths.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-paths.cmake
+/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig-version.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig-version.cmake
+/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig.cmake
+/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtc-msg-extras.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtc-msg-extras.cmake
+/home/matteo/reachability/devel/.private/mtc/share/roseus/ros/mtc/manifest.l /home/matteo/reachability/devel/share/roseus/ros/mtc/manifest.l
+/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
+/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/base_routine /home/matteo/reachability/devel/lib/mtc/base_routine
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/moveit_grasps_grasp_pipeline /home/matteo/reachability/devel/lib/mtc/moveit_grasps_grasp_pipeline
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace
+/home/matteo/reachability/devel/.private/mtc/lib/python3/dist-packages/mtc/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/mtc/__init__.py
+/home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc
diff --git a/.catkin_tools/profiles/default/packages/mtc/package.xml b/.catkin_tools/profiles/default/packages/mtc/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1213557c52d3af5afd9af5b3363c849eb40c63af
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/mtc/package.xml
@@ -0,0 +1,54 @@
+<package>
+  <name>mtc</name>
+  <version>2.0.0</version>
+  <description>
+    A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline.
+    Does not consider friction cones or other dynamics.
+  </description>
+  <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer>
+  <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url>
+  <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url>
+  <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url>
+
+  <author email="davetcoleman@gmail.com">Dave Coleman</author>
+  <author email="mcevoy.andy@gmail.com">Andy McEvoy</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>eigen_conversions</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf_conversions</build_depend>
+  <build_depend>moveit_ros_planning</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_visual_tools</build_depend>
+  <build_depend>rosparam_shortcuts</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>YAML_CPP</build_depend> 
+
+  <run_depend>YAML_CPP</run_depend> 
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>moveit_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>moveit_visual_tools</run_depend>
+  <run_depend>rosparam_shortcuts</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt b/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6142ce29e188e7f9009a999cd3a4eb59f02ce93a
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/panda_moveit_config2/devel_manifest.txt
@@ -0,0 +1,5 @@
+panda_moveit_config2
+/home/matteo/reachability/devel/.private/panda_moveit_config2/cmake.lock /home/matteo/reachability/devel/./cmake.lock
+/home/matteo/reachability/devel/.private/panda_moveit_config2/share/panda_moveit_config2/cmake/panda_moveit_config2Config.cmake /home/matteo/reachability/devel/share/panda_moveit_config2/cmake/panda_moveit_config2Config.cmake
+/home/matteo/reachability/devel/.private/panda_moveit_config2/share/panda_moveit_config2/cmake/panda_moveit_config2Config-version.cmake /home/matteo/reachability/devel/share/panda_moveit_config2/cmake/panda_moveit_config2Config-version.cmake
+/home/matteo/reachability/devel/.private/panda_moveit_config2/lib/pkgconfig/panda_moveit_config2.pc /home/matteo/reachability/devel/lib/pkgconfig/panda_moveit_config2.pc
diff --git a/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml b/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..ad33dee20a6f0b7c72101dfc259cc343b08f1924
--- /dev/null
+++ b/.catkin_tools/profiles/default/packages/panda_moveit_config2/package.xml
@@ -0,0 +1,32 @@
+<package>
+  <name>panda_moveit_config2</name>
+  <version>0.7.3</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the panda with the MoveIt! Motion Planning Framework
+  </description>
+  <author email="mike@picknik.ai">Mike Lautman</author>
+  <maintainer email="mike@picknik.ai">Mike Lautman</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>franka_description</run_depend>
+  <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
+  <run_depend>moveit_kinematics</run_depend>
+  <run_depend>moveit_planners_ompl</run_depend>
+  <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_state_publisher_gui</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>topic_tools</run_depend>
+  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment.
+       Commented the dependency until this works. -->
+  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
+</package>
diff --git a/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt b/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt
index acce48a20d50a582181471c6364ca1bf648a984f..bd2e016b375d9c3743cef1ccd43fa18d830afa80 100644
--- a/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt
+++ b/.catkin_tools/profiles/default/packages/reachability/devel_manifest.txt
@@ -6,7 +6,6 @@ reachability
 /home/matteo/reachability/devel/.private/reachability/share/reachability/cmake/reachability-msg-paths.cmake /home/matteo/reachability/devel/share/reachability/cmake/reachability-msg-paths.cmake
 /home/matteo/reachability/devel/.private/reachability/share/roseus/ros/reachability/manifest.l /home/matteo/reachability/devel/share/roseus/ros/reachability/manifest.l
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/map_analyse /home/matteo/reachability/devel/lib/reachability/map_analyse
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/publisher_node /home/matteo/reachability/devel/lib/reachability/publisher_node
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/visualization /home/matteo/reachability/devel/lib/reachability/visualization
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/single_pick /home/matteo/reachability/devel/lib/reachability/single_pick
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/reach_map /home/matteo/reachability/devel/lib/reachability/reach_map
@@ -14,6 +13,5 @@ reachability
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/pick_and_place /home/matteo/reachability/devel/lib/reachability/pick_and_place
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/task_viewer /home/matteo/reachability/devel/lib/reachability/task_viewer
 /home/matteo/reachability/devel/.private/reachability/lib/reachability/eval_analyse /home/matteo/reachability/devel/lib/reachability/eval_analyse
-/home/matteo/reachability/devel/.private/reachability/lib/reachability/base_position /home/matteo/reachability/devel/lib/reachability/base_position
 /home/matteo/reachability/devel/.private/reachability/lib/python3/dist-packages/reachability/__init__.py /home/matteo/reachability/devel/lib/python3/dist-packages/reachability/__init__.py
 /home/matteo/reachability/devel/.private/reachability/lib/pkgconfig/reachability.pc /home/matteo/reachability/devel/lib/pkgconfig/reachability.pc
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index c2e1bd9bb62f5f6138ada867504d0e20d660cdad..6842f49097f10bbd82d966dea0e104d77f31cc8a 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -1,19 +1,20 @@
 {
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "",
-        "limitSymbolsToIncludedHeaders": true
-      },
-      "includePath": [
-        "/opt/ros/noetic/include/**",
-        "/home/matteo/catkin_ws/devel/include/**",
-        "/home/matteo/catkin_ws/src/beginner_tutorials/include/**",
-        "/home/matteo/catkin_ws/src/komplette_zeitverschwendung/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS"
-    }
-  ],
-  "version": 4
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "",
+                "limitSymbolsToIncludedHeaders": true
+            },
+            "includePath": [
+                "/opt/ros/noetic/include/**",
+                "/home/matteo/catkin_ws/devel/include/**",
+                "/home/matteo/catkin_ws/src/beginner_tutorials/include/**",
+                "/home/matteo/catkin_ws/src/komplette_zeitverschwendung/include/**",
+                "/home/matteo/reachability/src/mtc/include/**",
+                "/usr/include/**"
+            ],
+            "name": "ROS"
+        }
+    ],
+    "version": 4
 }
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
index e0f87089cf20993e636bd9ad82960f91554fcbbd..40057ab04b1b13167eb38ff9d40fcafacad8c57e 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -79,7 +79,8 @@
         "variant": "cpp",
         "bit": "cpp",
         "valarray": "cpp",
-        "regex": "cpp"
+        "regex": "cpp",
+        "any": "cpp"
     },
     "cmake.sourceDirectory": "${workspaceFolder}/src"
 }
\ No newline at end of file
diff --git a/src/.rosinstall b/src/.rosinstall
new file mode 100644
index 0000000000000000000000000000000000000000..97dd12a43027c4fa0a399abf615865e047d2c168
--- /dev/null
+++ b/src/.rosinstall
@@ -0,0 +1 @@
+# THIS IS AN AUTOGENERATED FILE, LAST GENERATED USING wstool ON 2022-07-25
diff --git a/src/moveit_grasps b/src/moveit_grasps
new file mode 160000
index 0000000000000000000000000000000000000000..e70b88f97ed223dbcaecd3343316b6900029dacd
--- /dev/null
+++ b/src/moveit_grasps
@@ -0,0 +1 @@
+Subproject commit e70b88f97ed223dbcaecd3343316b6900029dacd
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..d65f948ecfbd023f06f919e83b0a10792dea856e
--- /dev/null
+++ b/src/mtc/CMakeLists.txt
@@ -0,0 +1,110 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mtc)
+# C++ 11
+add_compile_options(-std=c++11)
+
+# Warnings
+add_definitions(-W -Wall -Wextra
+  -Wwrite-strings -Wunreachable-code -Wpointer-arith
+  -Winit-self -Wredundant-decls
+  -Wno-unused-parameter -Wno-unused-function)
+
+# System dependencies are found with CMake's conventions
+find_package(Eigen3 REQUIRED)
+find_package(Boost REQUIRED thread system)
+find_package(yaml-cpp REQUIRED)
+
+find_package(OpenMP)
+
+# Load catkin and all dependencies required for this package
+find_package(catkin REQUIRED COMPONENTS
+  eigen_conversions
+  geometry_msgs
+  message_generation
+  moveit_core
+  moveit_msgs
+  moveit_ros_planning
+  moveit_ros_planning_interface
+  moveit_visual_tools
+  roscpp
+  roslint
+  rosparam_shortcuts
+  std_msgs
+  tf
+  tf_conversions
+  trajectory_msgs
+)
+
+# Catkin
+catkin_package(
+  LIBRARIES
+    moveit_grasps
+    moveit_grasps_filter
+  CATKIN_DEPENDS
+    geometry_msgs
+    message_runtime
+    moveit_msgs
+    moveit_visual_tools
+    rosparam_shortcuts
+    std_msgs
+    trajectory_msgs
+  INCLUDE_DIRS
+    include
+  DEPENDS
+    EIGEN3
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+# Grasp Library
+add_library(moveit_grasps
+  src/grasp_candidate.cpp
+  src/grasp_data.cpp
+  src/grasp_generator.cpp
+  src/grasp_scorer.cpp
+  src/two_finger_grasp_data.cpp
+  src/two_finger_grasp_generator.cpp
+  src/two_finger_grasp_scorer.cpp
+)
+target_link_libraries(moveit_grasps
+  ${catkin_LIBRARIES} ${Boost_LIBRARIES}
+)
+
+# Grasp Filter Library
+add_library(moveit_grasps_filter
+  src/grasp_filter.cpp
+  src/two_finger_grasp_filter.cpp
+  src/grasp_planner.cpp
+)
+target_link_libraries(moveit_grasps_filter
+  moveit_grasps
+  ${catkin_LIBRARIES}
+  ${Boost_LIBRARIES}
+)
+set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") # for threading
+set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
+
+# Demo grasp pipeline
+add_executable(base_routine src/base_routine.cpp)
+add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.cpp)
+
+
+add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(moveit_grasps_grasp_pipeline
+moveit_grasps moveit_grasps_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} yaml-cpp
+)
+
+target_link_libraries(base_routine 
+  ${catkin_LIBRARIES}
+  ${OCTOMAP_LIBRARIES}
+  yaml-cpp
+)
diff --git a/src/mtc/config/moveit_grasps_config.yaml b/src/mtc/config/moveit_grasps_config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..38c33d08e174a8c7c34951a0c12ec633c8e47a32
--- /dev/null
+++ b/src/mtc/config/moveit_grasps_config.yaml
@@ -0,0 +1,55 @@
+moveit_grasps:
+  ####################################
+  # These settings are used by the grasp generator which generates GraspCandidates
+  ####################################
+  generator:
+    # Other verbose stuff
+    verbose: true
+
+    # Show hand of generated grasps
+    show_prefiltered_grasps: true
+    show_prefiltered_grasps_speed: 0.001  # (sec / grasp)
+
+    # --------------------------------
+    # Finger gripper specific settings
+    # --------------------------------
+    # None yet
+
+    # --------------------------------
+    # Suction gripper specific settings
+    # --------------------------------
+    # Visual debug options
+    debug_top_grasps: false
+    # Show suction gripper overhang
+    show_grasp_overhang: false
+
+  ####################################
+  # The grasp filter filters out unreachable GraspCandidates and populates valid GraspCandidates' pregrasp_ik_solution_ and grasp_ik_solution_'s
+  ####################################
+  filter:
+    # Show summary statistics
+    statistics_verbose: true
+    # Visualize the planes used by the filter
+    show_cutting_planes: true
+    # Collision checking in verbose. Note: this will cause filtering to happen on a single thread rather than in parallel
+    collision_verbose: true
+    collision_verbose_speed: 0.01  # (sec/ solution)
+    # Collision checking in verbose on second pass (after all failed)
+    show_grasp_filter_collision_if_failed: true
+    # Show post-filter arrows
+    show_filtered_grasps: true
+    # Show pose-filter arm ik solutions
+    show_filtered_arm_solutions: true
+    show_filtered_arm_solutions_pregrasp_speed: 0.01  # (sec / solution)
+    show_filtered_arm_solutions_speed: 0.01  # (sec / solution)
+
+  ####################################
+  # The GraspPlanner generates approach, lift and retreat paths for a GraspCandidate.
+  # If the GraspPlanner is unable to plan 100% of the approach path and at least ~90% of the lift and retreat paths, then it considers the GraspCandidate to be infeasible
+  ####################################
+  planner:
+    # Cartesian planning
+    statistics_verbose: true
+    verbose_cartesian_filtering: false
+    show_cartesian_waypoints: false
+    collision_checking_verbose: false
diff --git a/src/mtc/config/panda_grasp_data.yaml b/src/mtc/config/panda_grasp_data.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..9c7f45cc20f6877a6a32a7c0e5762ccb40e32db3
--- /dev/null
+++ b/src/mtc/config/panda_grasp_data.yaml
@@ -0,0 +1,88 @@
+base_link: 'world'
+# =====================================================
+# Note: these parameters are copied from Robotiq gripper without tweaking
+hand:
+    end_effector_name: 'hand'  # ee group name
+    end_effector_type: 'finger'  # 'finger' or 'suction'
+
+    # x/y translation and angle orientation grasp resolution parameters used for sampling grasp candidates
+    grasp_resolution : 1
+    angle_resolution : 90    # degrees
+
+    # min/max values of the grasp depth range in meter.
+    # This should fit to the distance from finger tip to inner palm or the suction cup stroke
+    grasp_max_depth : 0.035
+    grasp_min_depth : 0.0
+    # grasp_depth_resolution is the discretization distance in meter used for sampling the grasp depth value.
+    # Idealy this should be a small fraction of the depth range (i.e. 0.005).
+    # If the value exceeds the depth distance range grasp_max_depth is used by default.
+    grasp_depth_resolution : 0.035
+
+    # A grasp motion is composed of three linear paths: approach, retreat, lift
+    # The values below define the desired distances of each path.
+    # Length of approach path (this is in addition to the grasp_max_depth)
+    approach_distance_desired: 0.05  # approach path (in addition to grasp_max_depth)
+    retreat_distance_desired: 0.05   # retreat path (in addition to grasp_max_depth)
+    lift_distance_desired: 0.01      # lift path (used as MINIMUM safety distance from surface)
+
+    # minimum padding on each side of the object on approach
+    grasp_padding_on_approach: 0.005  # meter
+
+    # We need to know the transform from the arm IK link to the grasp point. By default we define the transform manually with
+    # `tcp_to_eef_mount_transform`. Alternatively, we can set this to a frame in the robot model and compute the transform
+    # automatically. this is done by setting `define_tcp_by_name: true` and providing a value for `tcp_name`.
+    #
+    # Distance from the eef mount to the palm of end effector [x, y, z, r, p, y]
+    # z-axis pointing toward object to grasp
+    # x-axis perp. to movement of grippers
+    # y-axis parallel to movement of grippers
+    # If we have a tool center point frame, we would set:
+    define_tcp_by_name: true
+    tcp_name: "tool_center_point"
+    # Alternatively, if we cannot add a frame for the tool center point, we can define tcp by transform
+    # tcp_to_eef_mount_transform :  [0, 0, -0.105, 0, 0, -0.7853] # NOTE: Imaginary point in the middle
+
+    # Target durations in seconds for reaching pregrasp and grasp positions in end effector motions
+    pregrasp_time_from_start : 0.0
+    grasp_time_from_start : 0.0
+
+    # Joint names and state values for the end effector.
+    # The names should match the values of pregrasp_posture and grasp_posture.
+    # These values can be left empty if end_effector_type is 'suction'
+    pregrasp_posture : [0.04]  # open position
+    grasp_posture : [0.0]      # close position
+    joints : ['panda_finger_joint1']
+
+    ##############################
+    ##  Finger gripper parameters
+    ##############################
+
+    # Distances between fingers matching to pregrasp_posture and grasp_posture, in meters
+    max_finger_width: 0.085
+    min_finger_width: 0.06
+
+    # Maximum allowed finger width for a grasp.
+    # This value should be considerably smaller than max_finger_width
+    # to allow padded collision checks
+    max_grasp_width: 0.08
+
+    # width of gripper fingers to ensure the finger tips fully overlap the object
+    gripper_finger_width : 0.015
+
+    ##############################
+    ## Suction gripper parameters
+    ##############################
+    # The width of the suction gripper along the specified axes. (Assumes that Z points in the direction of the suction cups)
+    active_suction_range_x : 0.22
+    active_suction_range_y : 0.12
+
+    # Some suction grippers can selectively enable and disable regions of suction. If you have more than one
+    # region that can be selectively activated or de-activated, set those values here and moveit_grasps
+    # will prioritize grasp candidates which use full suction regions rather than partial regions.
+    # it will also set the suction_regions_in_contact_ variable in the grasp candidates so that you are able
+    # to only enable suction on regions that are in contact with the grasp target.
+    # Note that this metric assumes that suction regions are equally sized
+    # ie. width = active_suction_range_x / suction_cols_count
+    #
+    suction_rows_count: 2
+    suction_cols_count: 2
diff --git a/src/mtc/descriptions/dummy.yaml b/src/mtc/descriptions/dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..a2fcd6d098bc55190970c7572ec152bd247c2d63
--- /dev/null
+++ b/src/mtc/descriptions/dummy.yaml
@@ -0,0 +1,284 @@
+task:
+  groups:
+    robot_x:
+      -
+        - -0.3
+        - -0.7
+        - 0.89
+      -
+        - -0.3
+        - -0.6
+        - 0.89
+      -
+        - -0.2
+        - -0.7
+        - 0.89
+      -
+        - -0.2
+        - -0.6
+        - 0.89
+      -
+        - 0.1
+        - -0.7
+        - 0.89
+      -
+        - 0.1
+        - -0.6
+        - 0.89
+      -
+        - 0.1
+        - -0.3
+        - 0.89
+      -
+        - 0.1
+        - -0.2
+        - 0.89
+      -
+        - 0.1
+        - -0.1
+        - 0.89
+      -
+        - 0.2
+        - -0.3
+        - 0.89
+      -
+        - 0.2
+        - -0.2
+        - 0.89
+      -
+        - 0.2
+        - -0.1
+        - 0.89
+      -
+        - 0.3
+        - -0.3
+        - 0.89
+      -
+        - 0.3
+        - -0.2
+        - 0.89
+      -
+        - 0.3
+        - -0.1
+        - 0.89
+      -
+        - 0.1
+        - 0.1
+        - 0.89
+      -
+        - 0.1
+        - 0.2
+        - 0.89
+      -
+        - 0.1
+        - 0.3
+        - 0.89
+      -
+        - 0.2
+        - 0.1
+        - 0.89
+      -
+        - 0.2
+        - 0.2
+        - 0.89
+      -
+        - 0.2
+        - 0.3
+        - 0.89
+      -
+        - 0.3
+        - 0.1
+        - 0.89
+      -
+        - 0.3
+        - 0.2
+        - 0.89
+      -
+        - 0.3
+        - 0.3
+        - 0.89
+      -
+        - -0.3
+        - 0.6
+        - 0.89
+      -
+        - -0.3
+        - 0.7
+        - 0.89
+      -
+        - -0.2
+        - 0.6
+        - 0.89
+      -
+        - -0.2
+        - 0.7
+        - 0.89
+      -
+        - 0.1
+        - 0.6
+        - 0.89
+      -
+        - 0.1
+        - 0.7
+        - 0.89
+    robot_y:
+      -
+        - 0.1
+        - 1.01
+        - 0.89
+      -
+        - 0.1
+        - 1.11
+        - 0.89
+      -
+        - 0.1
+        - 1.21
+        - 0.89
+      -
+        - 0.2
+        - 1.01
+        - 0.89
+      -
+        - 0.2
+        - 1.11
+        - 0.89
+      -
+        - 0.2
+        - 1.21
+        - 0.89
+      -
+        - 0.3
+        - 1.01
+        - 0.89
+      -
+        - 0.3
+        - 1.11
+        - 0.89
+      -
+        - 0.3
+        - 1.21
+        - 0.89
+      -
+        - 0.1
+        - 1.41
+        - 0.89
+      -
+        - 0.1
+        - 1.51
+        - 0.89
+      -
+        - 0.1
+        - 1.61
+        - 0.89
+      -
+        - 0.2
+        - 1.41
+        - 0.89
+      -
+        - 0.2
+        - 1.51
+        - 0.89
+      -
+        - 0.2
+        - 1.61
+        - 0.89
+      -
+        - 0.3
+        - 1.41
+        - 0.89
+      -
+        - 0.3
+        - 1.51
+        - 0.89
+      -
+        - 0.3
+        - 1.61
+        - 0.89
+      -
+        - -0.3
+        - 1.91
+        - 0.89
+      -
+        - -0.3
+        - 2.01
+        - 0.89
+      -
+        - -0.2
+        - 1.91
+        - 0.89
+      -
+        - -0.2
+        - 2.01
+        - 0.89
+      -
+        - 0.1
+        - 1.91
+        - 0.89
+      -
+        - 0.1
+        - 2.01
+        - 0.89
+      -
+        - 0
+        - 0.6525
+        - 1.49
+      -
+        - 0.1
+        - -0.7
+        - 0.9355
+      -
+        - 0.2
+        - 0.3
+        - 0.9355
+      -
+        - 0.2
+        - -0.1
+        - 0.9355
+      -
+        - 0.2
+        - -0.3
+        - 0.9355
+      -
+        - 0.1
+        - 1.91
+        - 0.9355
+      -
+        - -0.3
+        - -0.6
+        - 0.9355
+      -
+        - 0.3
+        - 1.41
+        - 0.9355
+      -
+        - -0.22
+        - 0
+        - 0.89
+      -
+        - -0.22
+        - 1.31
+        - 0.89
+      -
+        - -0.3
+        - 0.6
+        - 0.89
+      -
+        - -0.3
+        - 0.7
+        - 0.89
+      -
+        - -0.2
+        - 0.6
+        - 0.89
+      -
+        - -0.2
+        - 0.7
+        - 0.89
+      -
+        - 0.1
+        - 0.6
+        - 0.89
+      -
+        - 0.1
+        - 0.7
+        - 0.89
\ No newline at end of file
diff --git a/src/mtc/include/grasp_candidate.h b/src/mtc/include/grasp_candidate.h
new file mode 100644
index 0000000000000000000000000000000000000000..e954038fbffbc54851d8e91b389edc0d72aa3ccb
--- /dev/null
+++ b/src/mtc/include/grasp_candidate.h
@@ -0,0 +1,142 @@
+#ifndef MOVEIT_GRASPS__GRASP_CANDIDATE_
+#define MOVEIT_GRASPS__GRASP_CANDIDATE_
+
+// ROS
+#include <ros/ros.h>
+#include <moveit_msgs/Grasp.h>
+
+// Grasping
+#include "../include/grasp_data.h"
+
+// MoveIt
+#include <moveit/robot_state/robot_state.h>
+#include <moveit_msgs/Grasp.h>
+
+namespace moveit_grasps
+{
+typedef std::vector<std::vector<moveit::core::RobotStatePtr> > GraspTrajectories;
+enum GraspTrajectorySegments
+{
+  APPROACH = 0,
+  LIFT = 1,
+  RETREAT = 2
+};
+
+struct GraspFilterCode
+{
+  enum
+  {
+    NOT_FILTERED = 0,
+    GRASP_FILTERED_BY_IK,             // Ik solution at grasp failed
+    GRASP_FILTERED_BY_CUTTING_PLANE,  // grasp pose is in an unreachable part of the environment (eg: behind a wall)
+    GRASP_FILTERED_BY_ORIENTATION,    // grasp pose is not desireable
+    GRASP_FILTERED_BY_IK_CLOSED,      // ik solution was fine with grasp opened, but failed with grasp closed
+    PREGRASP_FILTERED_BY_IK,          // Ik solution before approach failed
+    GRASP_INVALID,                    // An error occured while processing the grasp
+    LAST                              // Used to track last value in the base class when inheriting
+  };
+};
+
+/**
+ * \brief Contains collected data for each potential grasp after it has been verified / filtered
+ *        This includes the pregrasp and grasp IK solution
+ */
+class GraspCandidate
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, const Eigen::Isometry3d& cuboid_pose);
+
+  bool getPreGraspState(moveit::core::RobotStatePtr& robot_state);
+
+  bool getGraspStateOpen(moveit::core::RobotStatePtr& robot_state);
+
+  bool getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state);
+
+  bool getGraspStateClosed(moveit::core::RobotStatePtr& robot_state);
+
+  bool getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state);
+
+  virtual bool isValid();
+
+  moveit_msgs::Grasp grasp_;
+
+  /*# Contents of moveit_msgs::Grasp for reference
+
+    # A name for this grasp
+    string id
+
+    # The internal posture of the hand for the pre-grasp
+    # only positions are used
+    trajectory_msgs/JointTrajectory pre_grasp_posture
+
+    # The internal posture of the hand for the grasp
+    # positions and efforts are used
+    trajectory_msgs/JointTrajectory grasp_posture
+
+    # The position of the end-effector for the grasp.  This is the pose of
+    # the "parent_link" of the end-effector, not actually the pose of any
+    # link *in* the end-effector.  Typically this would be the pose of the
+    # most distal wrist link before the hand (end-effector) links began.
+    geometry_msgs/PoseStamped grasp_pose
+
+    # The estimated probability of success for this grasp, or some other
+    # measure of how "good" it is.
+    float64 grasp_quality
+
+    # The approach direction to take before picking an object
+    GripperTranslation pre_grasp_approach
+    {
+       # defines a translation for the gripper, used in pickup or place tasks
+       # for example for lifting an object off a table or approaching the table for placing
+
+       # the direction of the translation
+       geometry_msgs/Vector3Stamped direction
+
+       # the desired translation distance
+       float32 desired_distance
+
+       # the min distance that must be considered feasible before the
+       # grasp is even attempted
+       float32 min_distance
+    }
+
+    # The retreat direction to take after a grasp has been completed (object is attached)
+    GripperTranslation post_grasp_retreat
+
+    # The retreat motion to perform when releasing the object; this information
+    # is not necessary for the grasp itself, but when releasing the object,
+    # the information will be necessary. The grasp used to perform a pickup
+    # is returned as part of the result, so this information is available for
+    # later use.
+    GripperTranslation post_place_retreat
+
+    # the maximum contact force to use while grasping (<=0 to disable)
+    float32 max_contact_force
+
+    # an optional list of obstacles that we have semantic information about
+    # and that can be touched/pushed/moved in the course of grasping
+    string[] allowed_touch_objects
+
+    # The minimum opening that the fingers can have as it approaches an object
+    float64 min_finger_open_on_approach
+  */
+
+  const GraspDataPtr grasp_data_;
+  // TODO(davetcoleman): possibly remove
+  Eigen::Isometry3d cuboid_pose_;  // pose of original object to grasp
+
+  int grasp_filtered_code_;  // All codes defined in enum
+
+  std::vector<double> grasp_ik_solution_;
+  std::vector<double> pregrasp_ik_solution_;
+
+  // Store pregrasp, grasp, lifted, and retreat trajectories
+  GraspTrajectories segmented_cartesian_traj_;
+};  // class
+
+typedef std::shared_ptr<GraspCandidate> GraspCandidatePtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_data.h b/src/mtc/include/grasp_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..874f1da15b6771408ca9afdef8fb9dd9e454ff86
--- /dev/null
+++ b/src/mtc/include/grasp_data.h
@@ -0,0 +1,102 @@
+#ifndef MOVEIT_GRASPS__GRASP_DATA_H_
+#define MOVEIT_GRASPS__GRASP_DATA_H_
+
+// Ros
+#include <ros/node_handle.h>
+
+// Msgs
+#include <geometry_msgs/Pose.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+// MoveIt
+#include <moveit/macros/class_forward.h>
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/robot_model/link_model.h>
+
+// moveit grasps
+#include "suction_voxel_matrix.h"
+
+namespace moveit_grasps
+{
+MOVEIT_CLASS_FORWARD(GraspData);
+
+// Map various arms to end effector grasp datas
+typedef std::map<const robot_model::JointModelGroup*, moveit_grasps::GraspDataPtr> GraspDatas;
+
+struct GraspData
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  /**
+   * \brief Loads grasp data from a yaml file (load from roslaunch)
+   * \param node handle - allows for namespacing
+   * \param end effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names
+   */
+  GraspData(const ros::NodeHandle& nh, const std::string& end_effector,
+            const moveit::core::RobotModelConstPtr& robot_model);
+
+  /**
+   * \brief Helper function for constructor
+   * \return true on success
+   */
+  virtual bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector);
+
+  /**
+   * \brief Alter a robot state so that the end effector corresponding to this grasp data is in pre-grasp state (OPEN)
+   * \param joint state of robot
+   * \return true on success
+   */
+  bool setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state);
+
+  /**
+   * \brief Alter a robot state so that the end effector corresponding to this grasp data is in grasp state (CLOSED)
+   * \param joint state of robot
+   * \return true on success
+   */
+  bool setRobotStateGrasp(robot_state::RobotStatePtr& robot_state);
+
+  /**
+   * \brief Alter a robot state so that the end effector corresponding to this grasp data is in a grasp posture
+   * \param joint state of robot
+   * \param posture - what state to set the end effector
+   * \return true on success
+   */
+  bool setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture);
+
+  /**
+   * \brief Debug data to console
+   */
+  virtual void print();
+
+  // The (possibly fictional) center point of a grasp
+  std::string tcp_name_;
+  Eigen::Isometry3d tcp_to_eef_mount_;  // Convert generic grasp pose to the parent arm's eef_mount frame of reference
+
+  trajectory_msgs::JointTrajectory pre_grasp_posture_;  // when the end effector is in "open" position
+  trajectory_msgs::JointTrajectory grasp_posture_;      // when the end effector is in "close" position
+  std::string base_link_;                               // name of global frame with z pointing up
+
+  const robot_model::JointModelGroup* ee_jmg_;   // this end effector
+  const robot_model::JointModelGroup* arm_jmg_;  // the arm that attaches to this end effector
+  const robot_model::RobotModelConstPtr robot_model_;
+
+  // Duplicate end effector data copied from RobotModel
+  // the last link in the kinematic chain before the end effector, e.g. "/gripper_roll_link" class
+  const robot_model::LinkModel* parent_link_;
+
+  int angle_resolution_;  // generate grasps at increments of: angle_resolution * pi / 180
+
+  double grasp_resolution_;
+  double grasp_depth_resolution_;  // generate grasps at this depth resolution along grasp_max_depth_
+  double grasp_min_depth_;         // minimum amount fingers must overlap object
+  double grasp_max_depth_;  // Maximum distance from tip of end effector inwords that an object can be for a grasp
+
+  // grasp approach and retreat parameters
+  double approach_distance_desired_;  // this is in addition to the grasp_max_depth
+  double retreat_distance_desired_;   // this is in addition to the grasp_max_depth
+  double lift_distance_desired_;
+  double grasp_padding_on_approach_;
+};
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_filter.h b/src/mtc/include/grasp_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3ece959f2e7e36132244f190ee37ebe7bfa98b1
--- /dev/null
+++ b/src/mtc/include/grasp_filter.h
@@ -0,0 +1,391 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Filters grasps based on kinematic feasibility and collision
+*/
+
+#ifndef MOVEIT_GRASPS__GRASP_FILTER_
+#define MOVEIT_GRASPS__GRASP_FILTER_
+
+// ROS
+#include <ros/ros.h>
+#include <tf_conversions/tf_eigen.h>
+#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseStamped.h>
+
+// Grasping
+#include "../include/grasp_generator.h"
+#include "../include/grasp_candidate.h"
+
+// Rviz
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+// MoveIt
+#include <moveit/robot_state/robot_state.h>
+#include <moveit_msgs/Grasp.h>
+#include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+
+// C++
+#include <boost/thread.hpp>
+#include <cmath>
+
+namespace moveit_grasps
+{
+enum GraspParallelPlane
+{
+  XY,
+  XZ,
+  YZ
+};
+
+/**
+ * \brief Contains information to filter grasps by a cutting plane
+ */
+struct CuttingPlane
+{
+  Eigen::Isometry3d pose_;
+  GraspParallelPlane plane_;
+  int direction_;
+
+  CuttingPlane(Eigen::Isometry3d pose, GraspParallelPlane plane, int direction)
+    : pose_(pose), plane_(plane), direction_(direction)
+  {
+  }
+};
+typedef std::shared_ptr<CuttingPlane> CuttingPlanePtr;
+
+/**
+ * \brief Contains information to filter grasps by orientation
+ */
+struct DesiredGraspOrientation
+{
+  Eigen::Isometry3d pose_;
+  double max_angle_offset_;
+
+  DesiredGraspOrientation(Eigen::Isometry3d pose, double max_angle_offset)
+    : pose_(pose), max_angle_offset_(max_angle_offset)
+  {
+  }
+};
+typedef std::shared_ptr<DesiredGraspOrientation> DesiredGraspOrientationPtr;
+
+/**
+ * \brief Struct for passing parameters to threads, for cleaner code
+ */
+struct IkThreadStruct
+{
+  IkThreadStruct(std::vector<GraspCandidatePtr>& grasp_candidates,  // the input
+                 const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform,
+                 std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver,
+                 const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug,
+                 std::size_t thread_id, const std::string& grasp_target_object_id)
+    : grasp_candidates_(grasp_candidates)
+    , planning_scene_(planning_scene::PlanningScene::clone(planning_scene))
+    , link_transform_(link_transform)
+    , grasp_id(grasp_id)
+    , kin_solver_(kin_solver)
+    , robot_state_(std::make_shared<robot_state::RobotState>(*robot_state))
+    , timeout_(timeout)
+    , filter_pregrasp_(filter_pregrasp)
+    , visual_debug_(visual_debug)
+    , thread_id_(thread_id)
+    , grasp_target_object_id_(grasp_target_object_id)
+  {
+  }
+
+  std::vector<GraspCandidatePtr>& grasp_candidates_;
+  planning_scene::PlanningScenePtr planning_scene_;
+  Eigen::Isometry3d link_transform_;
+  std::size_t grasp_id;
+  kinematics::KinematicsBaseConstPtr kin_solver_;
+  robot_state::RobotStatePtr robot_state_;
+  double timeout_;
+  bool filter_pregrasp_;
+  bool visual_debug_;
+  std::size_t thread_id_;
+  // The name of the target grasp object to be used in the planning scene.
+  // NOTE: The ik filtering checks if the user has already added the grasp object
+  // to the planning scene. If it hasn't then GraspFilter will not modify the planning scene allowed collision matrix
+  std::string grasp_target_object_id_;
+
+  // Used within processing function
+  geometry_msgs::PoseStamped ik_pose_;  // Set from grasp candidate
+  std::vector<double> ik_seed_state_;
+};
+typedef std::shared_ptr<IkThreadStruct> IkThreadStructPtr;
+
+class GraspFilter
+{
+public:
+  // Constructor
+  GraspFilter(const robot_state::RobotStatePtr& robot_state,
+              const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
+
+  /**
+   * \brief Return grasps that are kinematically feasible
+   * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
+   * \param arm_jmg - the arm to solve the IK problem on
+   * \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position
+   * \param target_object_id - The name of the target grasp object in the planning scene if it exists
+   * \return true on successful filter
+   */
+  virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
+                            const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                            const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state,
+                            bool filter_pregrasp = false, const std::string& target_object_id = "");
+
+  virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
+                            const planning_scene::PlanningScenePtr& planning_scene,
+                            const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state,
+                            bool filter_pregrasp = false, const std::string& target_object_id = "");
+  /**
+   * \brief Return grasps that are kinematically feasible
+   * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
+   * \param arm_jmg - the arm to solve the IK problem on
+   * \param seed_state - A robot state to be used for IK. Ideally this will be close to the desired goal configuration.
+   * \param filter_pregrasp - Whether to also check ik feasibility for the pregrasp position
+   * \param visualize - visualize IK filtering
+   * \param target_object_id - The name of the target grasp object in the planning scene if it exists
+   * \return true on successful filter
+   */
+  virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                         const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                         const robot_model::JointModelGroup* arm_jmg,
+                                         const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
+                                         bool visualize, const std::string& grasp_target_object_id = "");
+
+  virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                         const planning_scene::PlanningScenePtr& planning_scene,
+                                         const robot_model::JointModelGroup* arm_jmg,
+                                         const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
+                                         bool visualize, const std::string& grasp_target_object_id = "");
+
+  /**
+   * \brief Print grasp filtering statistics
+   */
+  virtual void printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const;
+
+  /**
+   * \brief Method for checking part of the possible grasps list. MUST BE THREAD SAFE
+   */
+  virtual bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct);
+
+  /**
+   * \brief Used for sorting an array of CandidateGrasps
+   * \return true if A is less than B
+   */
+  static bool compareGraspScores(const GraspCandidatePtr& grasp_a, const GraspCandidatePtr& grasp_b)
+  {
+    // Determine if A or B has higher quality
+    return (grasp_a->grasp_.grasp_quality > grasp_b->grasp_.grasp_quality);
+  }
+
+  /**
+   * \brief Of an array of grasps, sort the valid ones from best score to worse score
+   * \return false if no grasps remain
+   */
+  bool removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const;
+
+  /**
+   * \brief Add a cutting plane filter for a shelf bin
+   * \return true on success
+   */
+  bool addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product,
+                              const double& bin_width, const double& bin_height);
+
+  /* \brief Set the ACM entry to ignore collisions between the ee_link_names and the object in the planning_scene */
+  void setACMFingerEntry(const std::string& object_name, bool allowed, const std::vector<std::string>& ee_link_names,
+                         const planning_scene::PlanningScenePtr& planning_scene);
+
+protected:
+  /**
+   * \brief Filter grasps by cutting plane
+   * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
+   * \param filter_pose - pose of filter that will define cutting plane
+   * \param plane - the cutting plane (XY, XZ, or YZ)
+   * \param direction - which side of this plane to cut (+/- 1)
+   * \return true if grasp is filtered by operation
+   */
+  bool filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose,
+                          GraspParallelPlane plane, int direction) const;
+
+  /**
+   * \brief Filter grasps by desired orientation. Think of reaching into a small opening, you can only rotate your hand
+   * a tiny amount and still grasp an object. If there's empty space behind an object, grasps behind the object aren't
+   * removed by the cutting plane operations. We know we'll never get to them because they deviate too much from the
+   * desired grasping pose... straight in.
+   * \param grasp_candidates - a grasp candidate that this will test.
+   * \param desired_pose - the desired grasp pose ( using standard grasping orientation )
+   * \param max_angular_offset - maximum angle allowed between the grasp pose and the desired pose
+   * \return true if grasp is filtered by operation
+   */
+  bool filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose,
+                                double max_angular_offset) const;
+
+  /**
+   * \brief Filter grasps by feasablity of the grasp pose.
+   * \param grasp_candidates - a grasp candidate that this will test
+   * \param grasp_ik_solution - the IK solution if one exists
+   * \param ik_thread_struct - a struct containing the planning_scene
+   * \return true if grasp is filtered by operation
+   */
+  bool filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution,
+                            const IkThreadStructPtr& ik_thread_struct) const;
+
+  /**
+   * \brief Filter grasps by feasablity of the pre grasp pose.
+   * \param grasp_candidates - a grasp candidate that this will test
+   * \param grasp_ik_solution - the IK solution if one exists
+   * \param ik_thread_struct - a struct containing the planning_scene
+   * \return true if grasp is filtered by operation
+   */
+  bool filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& pregrasp_ik_solution,
+                               const IkThreadStructPtr& ik_thread_struct) const;
+
+  /**
+   * \brief Helper for the thread function to find IK solutions
+   * \return true on success
+   */
+  bool findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
+                      const GraspCandidatePtr& grasp_candidate,
+                      const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const;
+
+  /**
+   * \brief add a cutting plane
+   * \param pose - pose describing the cutting plane
+   * \param plane - which plane to use as the cutting plane
+   * \param direction - on which side of the plane the grasps will be removed
+   */
+  void addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction);
+
+  /**
+   * \brief Show all cutting planes that are currently enables
+   * \return true on success
+   */
+  bool visualizeCuttingPlanes();
+
+  /**
+   * \brief clear all cutting planes
+   */
+  void clearCuttingPlanes();
+
+  /**
+   * \brief add a desired grasp orientation
+   * \param pose - the desired grasping pose
+   * \param max_angle_offset - maximum amount a generated grasp can deviate from the desired pose
+   */
+  void addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset);
+
+  /**
+   * \brief clear all desired orientations
+   */
+  void clearDesiredGraspOrientations();
+
+  /**
+   * \brief Show grasps after being filtered
+   * \return true on success
+   */
+  bool visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                       const moveit::core::JointModelGroup* arm_jmg);
+
+  /**
+   * \brief Show IK solutions of entire arm
+   * \return true on success
+   */
+  bool visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                            const moveit::core::JointModelGroup* arm_jmg, double animation_speed);
+
+  /**
+   * \brief Show solutions of entire arm
+   * \return true on success
+   */
+  bool visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates);
+
+  /**
+   * \brief Show an entire planning scene
+   */
+  void publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const;
+
+protected:
+  // logging name
+  const std::string name_;
+
+  // Allow a writeable robot state
+  robot_state::RobotStatePtr robot_state_;
+
+  // Keep a robot state for every thread
+  std::vector<robot_state::RobotStatePtr> robot_states_;
+
+  // Threaded kinematic solvers
+  std::map<std::string, std::vector<kinematics::KinematicsBaseConstPtr> > kin_solvers_;
+
+  // Class for publishing stuff to rviz
+  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
+  // for rviz visualization of the planning scene
+  ros::Publisher planning_scene_publisher_;
+
+  // Number of degrees of freedom for the IK solver to find
+  std::size_t num_variables_;
+
+  // Time to allow IK solver to run
+  double solver_timeout_;
+
+  // Visualization levels
+  bool collision_verbose_;
+  bool statistics_verbose_;
+  double collision_verbose_speed_;
+  bool show_filtered_grasps_;
+  bool show_filtered_arm_solutions_;
+  bool show_cutting_planes_;
+  double show_filtered_arm_solutions_speed_;
+  double show_filtered_arm_solutions_pregrasp_speed_;
+  bool show_grasp_filter_collision_if_failed_;
+
+  // Shared node handle
+  ros::NodeHandle nh_;
+
+  // Cutting planes and orientation filter
+  std::vector<CuttingPlanePtr> cutting_planes_;
+  std::vector<DesiredGraspOrientationPtr> desired_grasp_orientations_;
+
+};  // end of class
+
+typedef std::shared_ptr<GraspFilter> GraspFilterPtr;
+typedef std::shared_ptr<const GraspFilter> GraspFilterConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_generator.h b/src/mtc/include/grasp_generator.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d8a3f55b7d4af2e6fbcc4610b98bd8d14d3bfee
--- /dev/null
+++ b/src/mtc/include/grasp_generator.h
@@ -0,0 +1,248 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
+   Desc:   Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
+*/
+
+#ifndef MOVEIT_GRASPS__GRASP_GENERATOR_H_
+#define MOVEIT_GRASPS__GRASP_GENERATOR_H_
+
+// ROS
+#include <ros/ros.h>
+
+// TF
+#include <tf_conversions/tf_eigen.h>
+
+// Msgs
+#include <geometry_msgs/PoseArray.h>
+
+// MoveIt
+#include <moveit_msgs/Grasp.h>
+
+// geometric_shapes
+#include <geometric_shapes/shape_operations.h>
+#include <geometric_shapes/bodies.h>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/Eigenvalues>
+#include <eigen_conversions/eigen_msg.h>
+
+// Visualization
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+// moveit_grasps
+#include "grasp_candidate.h"
+#include "grasp_scorer.h"
+
+// bounding_box
+//#include <bounding_box/bounding_box.h>
+
+// C++
+#include <cstdlib>
+#include <string>
+#include <cmath>
+#include <limits>
+
+#include "grasp_data.h"
+
+namespace moveit_grasps
+{
+static const double RAD2DEG = 57.2957795;
+static const double MIN_GRASP_DISTANCE = 0.001;  // m between grasps
+
+// Grasp axis orientation
+enum grasp_axis_t
+{
+  X_AXIS,
+  Y_AXIS,
+  Z_AXIS
+};
+
+class GraspGenerator
+{
+public:
+  // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles).
+  // With GCC, this is done with a attribute ((aligned(16))).
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Constructor
+   */
+  GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false);
+
+  /**
+   * \brief Create possible grasp positions around a cuboid
+   * \param cuboid_pose - centroid of object to grasp in world frame
+   * \param depth length of cuboid along local x-axis
+   * \param width length of cuboid along local y-axis
+   * \param height length of cuboid along local z-axis
+   * \param grasp_data data describing end effector
+   * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types
+   * \param grasp_candidates possible grasps generated
+   * \return true if successful
+   */
+  virtual bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                              const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) = 0;
+
+  /**
+   * \brief Get the grasp direction vector relative to the world frame
+   * \param grasp
+   * \param name of parent link
+   * \return the approach direction
+   */
+  static Eigen::Vector3d getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link);
+  //  static Eigen::Vector3d getPostGraspDirection(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link);
+
+  /**
+   * \brief Using an input grasp description, get the pregrasp pose. The pregrasp pose is the grasp pose translated
+   * backwards in the grasp frame away from the object being grasped.
+   * \param grasp
+   * \param name of parent link
+   * \return pregrasp pose
+   */
+  static geometry_msgs::PoseStamped getPreGraspPose(const GraspCandidatePtr& grasp_candidate,
+                                                    const std::string& ee_parent_link);
+  /**
+   * \brief Compute the pre-grasp, grasp, lift and retreat poses for a grasp candidate
+   * \param grasp_candidate - the grasp candidate
+   * \param grasp_waypoints - a reference to a vector that will be populated with the pre-grasp, grasp, lift and retreat
+   * poses in that order.
+   */
+  static void getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, EigenSTL::vector_Isometry3d& grasp_waypoints);
+
+  /**
+   * \brief Helper to convert a robot-specific grasp to an arrow pointed in the right direction
+   * \param grasp - the grasp to show
+   * \param arm - the planning group of the arm we want to display
+   * \return true on success
+   */
+  void publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data,
+                         const rviz_visual_tools::colors& color, double approach_length = 0.1);
+
+  /**
+   * \brief Getter for Verbose
+   */
+  bool getVerbose()
+  {
+    return verbose_;
+  }
+
+  /**
+   * \brief Getter for ideal grasp pose
+   */
+  Eigen::Isometry3d getIdealTCPGraspPose()
+  {
+    return ideal_grasp_pose_;
+  }
+
+  /**
+   * \brief Setter for ideal grasp pose for scoring
+   */
+  void setIdealTCPGraspPose(Eigen::Isometry3d ideal_pose)
+  {
+    ideal_grasp_pose_ = ideal_pose;
+  }
+
+  /**
+   * \brief Setter for the roll pitch yall ideal grasp pose for scoring
+   */
+  void setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy);
+
+  /**
+   * \brief Setter for grasp score weights
+   */
+  void setGraspScoreWeights(const GraspScoreWeightsPtr& grasp_score_weights)
+  {
+    grasp_score_weights_ = std::make_shared<GraspScoreWeights>(*grasp_score_weights);
+  }
+
+  /**
+   * \brief Setter for grasp score weights
+   */
+  const GraspScoreWeightsPtr& getGraspScoreWeights()
+  {
+    return grasp_score_weights_;
+  }
+
+  /**
+   * \brief Setter for Verbose
+   */
+  void setVerbose(bool verbose)
+  {
+    verbose_ = verbose;
+  }
+
+  /**
+   * \brief Visualize animated grasps
+   * \return true on success
+   */
+  bool visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                               const moveit::core::JointModelGroup* ee_jmg, double animation_speed);
+
+  // Ideal grasp pose for scoring purposes
+  Eigen::Isometry3d ideal_grasp_pose_;
+
+protected:
+  // class for publishing stuff to rviz
+  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
+
+  // Display more output both in console
+  bool verbose_;
+
+  // Visual debug settings
+  bool show_prefiltered_grasps_;
+  double show_prefiltered_grasps_speed_;
+
+  // Shared node handle
+  ros::NodeHandle nh_;
+
+  // Transform from frame of box to global frame
+  Eigen::Isometry3d object_global_transform_;
+
+  double min_grasp_distance_, max_grasp_distance_;
+  Eigen::Vector3d min_translations_, max_translations_;
+
+  GraspScoreWeightsPtr grasp_score_weights_;
+
+};  // end of class
+
+typedef std::shared_ptr<GraspGenerator> GraspGeneratorPtr;
+typedef std::shared_ptr<const GraspGenerator> GraspGeneratorConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_planner copy.h b/src/mtc/include/grasp_planner copy.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5271665b775d7b2a073298ef58f5eb2754db64e
--- /dev/null
+++ b/src/mtc/include/grasp_planner copy.h	
@@ -0,0 +1,163 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
+*/
+
+#ifndef MOVEIT_GRASPS__GRASP_PLANNER_
+#define MOVEIT_GRASPS__GRASP_PLANNER_
+
+// ROS
+#include <ros/ros.h>
+
+// moveit_grasps
+#include "../include/grasp_candidate.h"
+#include "../include/grasp_generator.h"
+
+// moveit
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+namespace moveit_grasps
+{
+// Allow an interrupt to be called that waits for user input, useful for debugging
+typedef boost::function<void(std::string message)> WaitForNextStepCallback;
+
+class GraspPlanner
+{
+public:
+  /**
+   * \brief Constructor
+   */
+  GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
+
+  /**
+   * \brief Plan entire cartesian manipulation sequence
+   * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths
+   * \param robot_state - robot_state to be used for computeCartesianPath
+   * \param planning_scene_monitor - Current state of the world
+   * \param grasp_data - robot gripper configuration
+   * \param grasp_object_id - The name of the target grasp object to be used in the planning scene.
+   * \return true on success
+   */
+  bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                  const robot_state::RobotStatePtr& robot_state,
+                                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                  const std::string& grasp_object_id = "");
+
+  bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                  const robot_state::RobotStatePtr& robot_state,
+                                  const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                  const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Plan entire cartesian manipulation sequence
+   * \param input - description
+   * \return true on success
+   */
+  bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
+                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                               bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
+
+  bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
+                               const planning_scene::PlanningSceneConstPtr& planning_scene,
+                               bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Compute a cartesian path along waypoints
+   * \return true on success
+   */
+  bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
+                                    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                    const moveit::core::RobotStatePtr& start_state,
+                                    const EigenSTL::vector_Isometry3d& waypoints,
+                                    const std::string& grasp_object_id = "");
+
+  bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
+                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                    const moveit::core::RobotStatePtr& start_state,
+                                    const EigenSTL::vector_Isometry3d& waypoints,
+                                    const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Wait for user input to proceeed
+   * \param message - text to display to user when waiting
+   */
+  void waitForNextStep(const std::string& message);
+
+  /**
+   * \brief Allow an interrupt to be called that waits for user input, useful for debugging
+   * \param message - text to display to user when waiting
+   */
+  // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach
+  void setWaitForNextStepCallback(WaitForNextStepCallback callback);
+
+  /**
+   * \brief Load parameter settings of the server under ~/debug_level
+   * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace
+   * \param setting_namespace - where on this node's namespace to load settings
+   *        e.g. /this_name/setting_namespace/some_parameter
+   * \return true on success
+   */
+  bool loadEnabledSettings();
+
+  /**
+   * \brief Check if a setting is enabled
+   * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace'
+   * \return true if setting is enabled
+   */
+  bool isEnabled(const std::string& setting_name);
+
+private:
+  // A shared node handle
+  ros::NodeHandle nh_;
+
+  // Class for publishing stuff to rviz
+  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
+
+  WaitForNextStepCallback wait_for_next_step_callback_;
+
+  // Visualization settings
+  bool enabled_settings_loaded_ = false;
+  std::map<std::string, bool> enabled_setting_;
+
+};  // end class
+
+// Create smart pointers for this class
+typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr;
+typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_planner.h b/src/mtc/include/grasp_planner.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b5d922f63884718faa38b90076d0b9df4bad31a
--- /dev/null
+++ b/src/mtc/include/grasp_planner.h
@@ -0,0 +1,163 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
+*/
+
+#ifndef MOVEIT_GRASPS__GRASP_PLANNER_
+#define MOVEIT_GRASPS__GRASP_PLANNER_
+
+// ROS
+#include <ros/ros.h>
+
+// moveit_grasps
+#include "grasp_candidate.h"
+#include "grasp_generator.h"
+
+// moveit
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+namespace moveit_grasps
+{
+// Allow an interrupt to be called that waits for user input, useful for debugging
+typedef boost::function<void(std::string message)> WaitForNextStepCallback;
+
+class GraspPlanner
+{
+public:
+  /**
+   * \brief Constructor
+   */
+  GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
+
+  /**
+   * \brief Plan entire cartesian manipulation sequence
+   * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths
+   * \param robot_state - robot_state to be used for computeCartesianPath
+   * \param planning_scene_monitor - Current state of the world
+   * \param grasp_data - robot gripper configuration
+   * \param grasp_object_id - The name of the target grasp object to be used in the planning scene.
+   * \return true on success
+   */
+  bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                  const robot_state::RobotStatePtr& robot_state,
+                                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                  const std::string& grasp_object_id = "");
+
+  bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                  const robot_state::RobotStatePtr& robot_state,
+                                  const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                  const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Plan entire cartesian manipulation sequence
+   * \param input - description
+   * \return true on success
+   */
+  bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
+                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                               bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
+
+  bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
+                               const planning_scene::PlanningSceneConstPtr& planning_scene,
+                               bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Compute a cartesian path along waypoints
+   * \return true on success
+   */
+  bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
+                                    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                    const moveit::core::RobotStatePtr& start_state,
+                                    const EigenSTL::vector_Isometry3d& waypoints,
+                                    const std::string& grasp_object_id = "");
+
+  bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
+                                    const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                    const moveit::core::RobotStatePtr& start_state,
+                                    const EigenSTL::vector_Isometry3d& waypoints,
+                                    const std::string& grasp_object_id = "");
+
+  /**
+   * \brief Wait for user input to proceeed
+   * \param message - text to display to user when waiting
+   */
+  void waitForNextStep(const std::string& message);
+
+  /**
+   * \brief Allow an interrupt to be called that waits for user input, useful for debugging
+   * \param message - text to display to user when waiting
+   */
+  // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach
+  void setWaitForNextStepCallback(WaitForNextStepCallback callback);
+
+  /**
+   * \brief Load parameter settings of the server under ~/debug_level
+   * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace
+   * \param setting_namespace - where on this node's namespace to load settings
+   *        e.g. /this_name/setting_namespace/some_parameter
+   * \return true on success
+   */
+  bool loadEnabledSettings();
+
+  /**
+   * \brief Check if a setting is enabled
+   * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace'
+   * \return true if setting is enabled
+   */
+  bool isEnabled(const std::string& setting_name);
+
+private:
+  // A shared node handle
+  ros::NodeHandle nh_;
+
+  // Class for publishing stuff to rviz
+  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
+
+  WaitForNextStepCallback wait_for_next_step_callback_;
+
+  // Visualization settings
+  bool enabled_settings_loaded_ = false;
+  std::map<std::string, bool> enabled_setting_;
+
+};  // end class
+
+// Create smart pointers for this class
+typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr;
+typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/grasp_scorer.h b/src/mtc/include/grasp_scorer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1b5f671ecdaa1c0799787ef439549911776db23
--- /dev/null
+++ b/src/mtc/include/grasp_scorer.h
@@ -0,0 +1,126 @@
+/*********************************************************************
+ * Software License Agreement ("Modified BSD License")
+ *
+ * Copyright (c) 2014, University of Colorado, Boulder
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Univ of CO, Boulder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+/**
+ * Authors : Andy McEvoy
+ * Desc    : Functions for scoring generated grasps
+ */
+
+#ifndef MOVEIT_GRASPS__GRASP_SCORER_
+#define MOVEIT_GRASPS__GRASP_SCORER_
+
+#include <cmath>
+
+#include <ros/ros.h>
+
+#include "../include/grasp_data.h"
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace moveit_grasps
+{
+struct GraspScoreWeights
+{
+  GraspScoreWeights()
+    : orientation_x_score_weight_(1.0)
+    , orientation_y_score_weight_(1.0)
+    , orientation_z_score_weight_(1.0)
+    , translation_x_score_weight_(1.0)
+    , translation_y_score_weight_(1.0)
+    , translation_z_score_weight_(1.0)
+  {
+  }
+
+  /* \brief Compute the weighted score given the orientation and translation scores */
+  double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores,
+                      bool verbose = false) const;
+
+  /* \brief returns the sum of the grasp score weights*/
+  virtual double getWeightTotal() const;
+
+  double orientation_x_score_weight_;
+  double orientation_y_score_weight_;
+  double orientation_z_score_weight_;
+  double translation_x_score_weight_;
+  double translation_y_score_weight_;
+  double translation_z_score_weight_;
+};
+// Create smart pointers for this struct
+typedef std::shared_ptr<GraspScoreWeights> GraspScoreWeightsPtr;
+
+class GraspScorer
+{
+public:
+  /**
+   * \brief Scores each axis of the grasp based on its angle to the desired pose axis.
+   * \param grasp_pose_tcp - the pose of the end effector
+   * \param ideal_pose - the ideal grasp pose (ex: straight into the bin)
+   * \return the unweighted scores:
+   *         1.0 -> 0 degrees between grasp axis and desired axis,
+   *         0.0 -> 180 degrees
+   */
+  static Eigen::Vector3d scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                   const Eigen::Isometry3d& ideal_pose);
+
+  /**
+   * \brief Score the grasp based on the translation values of the grasp pose
+   * \param grasp_pose_tcp - the pose of the end effector (not the eef mount)
+   * \param min_translations - the minimum translation values for all grasp poses
+   * \param max_translations - the maximum translation values for all grasp poses
+   * \return the unweighted scores:
+   *         0.0 -> pose is at the minimum translation in that axis
+   *         1.0 -> pose is at the maximum translation in that axis
+   */
+  static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
+                                               const Eigen::Vector3d& min_translations,
+                                               const Eigen::Vector3d& max_translations);
+
+  /**
+   * \brief Score the grasp based on the translation values of the grasp pose
+   * \param grasp_pose_tcp - the pose of the end effector (not the eef mount)
+   * \param ideal_pose - the ideal pose location
+   * \param object_pose - the pose of the object being grasped
+   * \param object_size - the size of the object represented as a vector [x,y,z]
+   * \param visual_tools - set to a moveit_visual_tools pointer to enable visual debugging
+   * \return the unweighted scores:
+   *         0.0 -> pose is at the ideal translation in that axis
+   */
+  static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
+                                               const Eigen::Isometry3d& ideal_pose);
+};
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/impl/abstract_map_loader.h b/src/mtc/include/impl/abstract_map_loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..58939fdb81de67a21f8d94f80fdbabc8a9c6855a
--- /dev/null
+++ b/src/mtc/include/impl/abstract_map_loader.h
@@ -0,0 +1,33 @@
+#ifndef ABSTRACT_MAP_LOADER_
+#define ABSTRACT_MAP_LOADER_
+
+#include "ros/ros.h"
+#include "geometry_msgs/Pose.h"
+#include "geometry_msgs/Point.h"
+#include <xmlrpcpp/XmlRpc.h>
+#include <tf2/LinearMath/Transform.h>
+
+#include "impl/abstract_strategy.h"
+
+
+class Abstract_map_loader{
+    private:
+    std::vector<geometry_msgs::Pose> _inv_map;
+    std::vector<std::vector<geometry_msgs::Point>> _targets_grasps;
+    Abstract_strategy *_strategy; 
+
+    public:
+    Abstract_map_loader() = default;
+    ~Abstract_map_loader() = default;
+
+    virtual void base_calculation()=0;
+    inline std::vector<geometry_msgs::Pose> get_inv_map() {return _inv_map;}
+    inline void set_inv_map(std::vector<geometry_msgs::Pose>& list) {_inv_map = list;}
+    inline void set_task(std::vector<std::vector<geometry_msgs::Point>>& lists_in_list) { _targets_grasps = lists_in_list;}
+    inline void set_strategy(Abstract_strategy* some_stratergy) {_strategy = some_stratergy;}
+    inline Abstract_strategy* get_strategy() {return _strategy;}
+
+};
+
+
+#endif
diff --git a/src/mtc/include/impl/abstract_strategy.h b/src/mtc/include/impl/abstract_strategy.h
new file mode 100644
index 0000000000000000000000000000000000000000..367fd55844322d21bf5d7e6a862f177cd3097a1f
--- /dev/null
+++ b/src/mtc/include/impl/abstract_strategy.h
@@ -0,0 +1,12 @@
+#ifndef ABSTRACT_STRATEGY_
+#define ABSTRACT_STRATEGY_
+
+#include <ros/ros.h>
+
+class Abstract_strategy {
+    public:
+    Abstract_strategy() = default;
+    ~Abstract_strategy() = default;
+    virtual void base_algorithm()=0;
+};
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/base_by_rotation.h b/src/mtc/include/impl/base_by_rotation.h
new file mode 100644
index 0000000000000000000000000000000000000000..2915cc105c49823621197fc368cf64afaf32ad9a
--- /dev/null
+++ b/src/mtc/include/impl/base_by_rotation.h
@@ -0,0 +1,16 @@
+#ifndef BASE_BY_ROTATION_
+#define BASE_BY_ROTATION_
+
+#include <ros/ros.h>
+
+#include "impl/abstract_strategy.h"
+
+class Base_by_rotation : public Abstract_strategy {
+    public:
+    Base_by_rotation()= default;
+    ~Base_by_rotation()= default;
+
+    void base_algorithm() override;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/map_loader.h b/src/mtc/include/impl/map_loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..8489227d335ea88619bd17fa67cb30efee42071a
--- /dev/null
+++ b/src/mtc/include/impl/map_loader.h
@@ -0,0 +1,18 @@
+#ifndef MAP_LOADER_
+#define MAP_LOADER_
+
+#include "ros/ros.h"
+#include "abstract_map_loader.h"
+#include "geometry_msgs/Pose.h"
+#include <regex>
+
+
+class Map_loader : public Abstract_map_loader {
+    public:
+    Map_loader(XmlRpc::XmlRpcValue& map_data, XmlRpc::XmlRpcValue& target_data);
+    void base_calculation() override;
+
+};
+
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/mtc2taskspace.h b/src/mtc/include/mtc2taskspace.h
new file mode 100644
index 0000000000000000000000000000000000000000..af645f13e38a7d4bd29c52fbe38366ada9f65d9c
--- /dev/null
+++ b/src/mtc/include/mtc2taskspace.h
@@ -0,0 +1,14 @@
+#ifndef MTC2TASKSPACE_H
+#define MTC2TASKSPACE_H
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include "std_msgs/String.h"
+#include <tf/transform_broadcaster.h>
+#include <regex>
+#include "yaml-cpp/yaml.h"
+#include <xmlrpcpp/XmlRpc.h>
+#include <fstream>
+
+
+#endif
diff --git a/src/mtc/include/state_validity_callback.h b/src/mtc/include/state_validity_callback.h
new file mode 100644
index 0000000000000000000000000000000000000000..575beb89ad141215474389e0b5fc6dd78f65b9d7
--- /dev/null
+++ b/src/mtc/include/state_validity_callback.h
@@ -0,0 +1,87 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Callback for checking if a state is in collision
+*/
+
+#ifndef MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK
+#define MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK
+
+// Rviz
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+// MoveIt
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/robot_state/robot_state.h>
+
+namespace
+{
+bool isGraspStateValid(const planning_scene::PlanningScene* planning_scene, bool visual_debug, double verbose_speed,
+                       const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools,
+                       robot_state::RobotState* robot_state, const robot_state::JointModelGroup* group,
+                       const double* ik_solution)
+{
+  robot_state->setJointGroupPositions(group, ik_solution);
+  robot_state->update();
+  if (!robot_state->satisfiesBounds(group))
+  {
+    ROS_DEBUG_STREAM_NAMED("is_grasp_state_valid", "Ik solution invalid");
+    return false;
+  }
+
+  if (!planning_scene)
+  {
+    ROS_ERROR_STREAM_NAMED("is_grasp_state_valid", "No planning scene provided");
+    return false;
+  }
+
+  if (!planning_scene->isStateColliding(*robot_state, group->getName()))
+    return true;  // not in collision
+
+  // Display more info about the collision
+  if (visual_debug && visual_tools)
+  {
+    visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED);
+    planning_scene->isStateColliding(*robot_state, group->getName(), true);
+    visual_tools->publishContactPoints(*robot_state, planning_scene);
+    visual_tools->trigger();
+    ros::Duration(verbose_speed).sleep();
+  }
+  return false;
+}
+
+}  // namespace
+
+#endif
diff --git a/src/mtc/include/suction_voxel_matrix.h b/src/mtc/include/suction_voxel_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..9bb35a6a59657097cdb1bef148e70ff80e378fb7
--- /dev/null
+++ b/src/mtc/include/suction_voxel_matrix.h
@@ -0,0 +1,188 @@
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L.
+ *     nor the names of its contributors may be used to endorse or
+ *     promote products derived from this software without specific
+ *     prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Authors: Mike Lautman
+   Description: Data class used by grasp data.
+*/
+
+#ifndef MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_
+#define MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_
+
+// ROS
+#include <ros/ros.h>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace moveit_grasps
+{
+struct SuctionVoxel
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  /* \brief
+    @param center_point - position of voxel center point in tcp frame
+    @param x_width - width of voxel along x dim in tcp frame
+    @param y_width - width of voxel along y dim in tcp frame
+   */
+  SuctionVoxel(const Eigen::Vector3d& center_point, double x_width, double y_width)
+    : center_point_(center_point), x_width_(x_width), y_width_(y_width)
+  {
+    top_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, y_width / 2.0, 0);
+    top_right_ = center_point + Eigen::Vector3d(x_width / 2.0, y_width / 2.0, 0);
+    bottom_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, -y_width / 2.0, 0);
+    bottom_right_ = center_point + Eigen::Vector3d(x_width / 2.0, -y_width / 2.0, 0);
+  }
+
+  // Voxel center point in tcp frame
+  Eigen::Vector3d center_point_;
+  double x_width_;
+  double y_width_;
+  // Voxel corners in tcp frame
+  Eigen::Vector3d top_left_;
+  Eigen::Vector3d top_right_;
+  Eigen::Vector3d bottom_left_;
+  Eigen::Vector3d bottom_right_;
+};
+
+class SuctionVoxelMatrix
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  SuctionVoxelMatrix(double suction_rows_count, double suction_cols_count, double total_suction_range_y,
+                     double total_suction_range_x)
+    : suction_rows_count_(suction_rows_count)
+    , suction_cols_count_(suction_cols_count)
+    , active_suction_range_x_(total_suction_range_x)
+    , active_suction_range_y_(total_suction_range_y)
+  {
+    voxel_x_width_ = active_suction_range_x_ / suction_cols_count_;
+    voxel_y_width_ = active_suction_range_y_ / suction_rows_count_;
+    suction_voxels_.resize(suction_rows_count_);
+    // We store the voxels starting bottom left and moving right then up
+    for (std::size_t voxel_y = 0; voxel_y < suction_rows_count_; ++voxel_y)
+    {
+      suction_voxels_[voxel_y].resize(suction_cols_count_);
+      for (std::size_t voxel_x = 0; voxel_x < suction_cols_count_; ++voxel_x)
+      {
+        suction_voxels_[voxel_y][voxel_x] = std::make_shared<SuctionVoxel>(
+            Eigen::Vector3d(-active_suction_range_x_ / 2.0 + voxel_x_width_ * (voxel_x + 0.5),
+                            -active_suction_range_y_ / 2.0 + voxel_y_width_ * (voxel_y + 0.5), 0),
+            voxel_x_width_, voxel_y_width_);
+      }
+    }
+  }
+
+  // \brief - get the voxel at the index location [row, col] with [0, 0] being the bottom left
+  bool getSuctionVoxel(std::size_t row, std::size_t col, std::shared_ptr<SuctionVoxel>& voxel)
+  {
+    if (row >= suction_rows_count_)
+    {
+      ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid row " << row << "/" << suction_rows_count_ - 1);
+      return false;
+    }
+
+    if (col >= suction_cols_count_)
+    {
+      ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid col " << col << "/" << suction_cols_count_ - 1);
+      return false;
+    }
+
+    voxel = suction_voxels_[row][col];
+    return true;
+  }
+
+  /** \brief Get the voxel at the index i where columns are the minor axis and rows are the major axes.
+   *  @param index - the index of the suction voxel where index / #cols is the row and index % #cols is the col
+   *                 index 0 is bottom left
+   */
+  bool getSuctionVoxel(std::size_t index, std::shared_ptr<SuctionVoxel>& voxel)
+  {
+    return getSuctionVoxel(index / suction_cols_count_, index % suction_cols_count_, voxel);
+  }
+
+  std::size_t getNumRows()
+  {
+    return suction_rows_count_;
+  }
+
+  std::size_t getNumCols()
+  {
+    return suction_cols_count_;
+  }
+
+  std::size_t getNumVoxels()
+  {
+    return suction_cols_count_ * suction_rows_count_;
+  }
+
+  double getVoxelArea()
+  {
+    return voxel_x_width_ * voxel_y_width_;
+  }
+
+  double getVoxelWidthX()
+  {
+    return voxel_x_width_;
+  }
+
+  double getVoxelWidthY()
+  {
+    return voxel_y_width_;
+  }
+
+  double getActiveSuctionWidthX()
+  {
+    return active_suction_range_x_;
+  }
+
+  double getActiveSuctionWidthY()
+  {
+    return active_suction_range_y_;
+  }
+
+protected:
+  std::size_t suction_rows_count_;
+  std::size_t suction_cols_count_;
+  double voxel_x_width_;
+  double voxel_y_width_;
+  double active_suction_range_x_;
+  double active_suction_range_y_;
+  std::vector<std::vector<std::shared_ptr<SuctionVoxel>>> suction_voxels_;
+};
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/two_finger_grasp_data.h b/src/mtc/include/two_finger_grasp_data.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b9a00f7493977e5c8bd2040b07eb83bdfdd6bfc
--- /dev/null
+++ b/src/mtc/include/two_finger_grasp_data.h
@@ -0,0 +1,120 @@
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L.
+ *     nor the names of its contributors may be used to endorse or
+ *     promote products derived from this software without specific
+ *     prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Authors: Dave Coleman, Bence Magyar
+   Description: Data class used by the grasp generator.
+*/
+
+#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_
+#define MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_
+
+// moveit grasps
+#include "../include/grasp_data.h"
+
+namespace moveit_grasps
+{
+MOVEIT_CLASS_FORWARD(TwoFingerGraspData);
+
+class TwoFingerGraspData : public GraspData
+{
+public:
+  /**
+   * \brief Creates a two finger grasp data object
+   * \param node_handle - allows for namespacing
+   * \param end_effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names
+   * \param robot_model - The robot model
+   */
+  TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector,
+                     const moveit::core::RobotModelConstPtr& robot_model);
+
+  /**
+   * \brief Helper function for constructor, loads grasp data from a yaml file (load from roslaunch)
+   * \param nh - node handle allows for namespacing
+   * \param end_effector - The end effector joint group name
+   * \return true on success
+   */
+  bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) override;
+
+  /**
+   * \brief Set the width between fingers as a percentage of object size and max finger width
+   * \param fraction_open - [0,1] the 0->closed 1->open
+   * \return true on success
+   */
+  bool setGraspWidth(double fraction_open, double min_finger_width, trajectory_msgs::JointTrajectory& grasp_posture);
+
+  /**
+   * \brief Convert width between fingers to joint positions
+   * \param distance_btw_fingers - (meters) The target distance between the fingers
+   * \param grasp_posture - output. A joint trajectory with the values for the end effector filled in.
+   * Note: we interpolate to get the joint value assuming a linear relationship between min and max
+   * finger distance and min and max finger joint value. This could likely be improved on a per-robot basis.
+   * \param end_effector - The end effector joint group name
+   * \return true on success
+   */
+  bool fingerWidthToGraspPosture(double distance_btw_fingers, trajectory_msgs::JointTrajectory& grasp_posture);
+
+  /**
+   * \brief Convert joint positions to full grasp posture
+   * \param joint_positions - the full joint state as a vector of doubles
+   * \param grasp_posture - output. The full grasp posture
+   * \return true on success
+   */
+  bool jointPositionsToGraspPosture(const std::vector<double>& joint_positions,
+                                    trajectory_msgs::JointTrajectory& grasp_posture);
+
+  /**
+   * \brief Debug data to console
+   */
+  void print() override;
+
+public:
+  /////////////////////////////////////
+  // Finger gripper specific parameters
+  /////////////////////////////////////
+  // Maximum allowed finger width for a grasp.
+  // This value should be considerably smaller than max_finger_width
+  // to allow padded collision checks
+  double max_grasp_width_;
+  // Maximum / Minimum distance between fingers
+  // For calculating the ratio between the distance between fingers and the joint values
+  double max_finger_width_;
+  double min_finger_width_;
+  // Parameter used to ensure generated grasps will overlap object
+  double gripper_finger_width_;
+};
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/two_finger_grasp_filter.h b/src/mtc/include/two_finger_grasp_filter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ec06ce7ee6e92956ecb24422cbcaf2541ac64a1
--- /dev/null
+++ b/src/mtc/include/two_finger_grasp_filter.h
@@ -0,0 +1,81 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Filters grasps based on kinematic feasibility and collision
+*/
+
+#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_
+#define MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_
+
+// Parent class
+#include "../include/grasp_filter.h"
+
+// Grasping
+#include "../include/two_finger_grasp_generator.h"
+
+namespace moveit_grasps
+{
+class TwoFingerGraspFilter : public GraspFilter
+{
+public:
+  // Constructor
+  TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state,
+                       const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
+
+  /**
+   * \brief Thread for checking part of the possible grasps list
+   */
+  bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override;
+
+  /**
+   * \brief Check if ik solution is in collision with fingers closed
+   * \return true on success
+   */
+  bool checkFingersClosedIK(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
+                            GraspCandidatePtr& grasp_candidate,
+                            const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const;
+
+protected:
+  // Name for logging
+  const std::string name_;
+
+};  // end of class
+
+typedef std::shared_ptr<TwoFingerGraspFilter> TwoFingerGraspFilterPtr;
+typedef std::shared_ptr<const TwoFingerGraspFilter> TwoFingerGraspFilterConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/two_finger_grasp_generator.h b/src/mtc/include/two_finger_grasp_generator.h
new file mode 100644
index 0000000000000000000000000000000000000000..454510aac1cef921e5677c09f7b3d09773738389
--- /dev/null
+++ b/src/mtc/include/two_finger_grasp_generator.h
@@ -0,0 +1,257 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
+   Desc:   Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
+*/
+
+#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_
+#define MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_
+
+#include "../include/grasp_generator.h"
+#include "../include/grasp_candidate.h"
+#include "../include/two_finger_grasp_scorer.h"
+#include "../include/two_finger_grasp_data.h"
+
+// Testing
+#include <gtest/gtest.h>
+
+namespace moveit_grasps
+{
+struct TwoFingerGraspCandidateConfig
+{
+  TwoFingerGraspCandidateConfig();
+  void enableAllGraspTypes();
+  void enableAllGraspAxes();
+  void enableAll();
+  void disableAllGraspTypes();
+  void disableAllGraspAxes();
+  void disableAll();
+
+  ///////////////////////////////
+  // Finger Gripper config values
+  ///////////////////////////////
+  bool enable_corner_grasps_;
+  bool enable_face_grasps_;
+  bool enable_variable_angle_grasps_;
+  bool enable_edge_grasps_;
+  bool generate_x_axis_grasps_;
+  bool generate_y_axis_grasps_;
+  bool generate_z_axis_grasps_;
+};
+
+class TwoFingerGraspGenerator : public GraspGenerator
+{
+public:
+  // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles).
+  // With GCC, this is done with a attribute ((aligned(16))).
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * \brief Constructor
+   */
+  TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false);
+
+  /* brief sets internal grasp_candidate_config_ variable
+   * \param grasp_candidate_config - a config describing the grasps to be generated
+   */
+  void setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config);
+
+  /**
+   * \brief Create possible grasp positions around a cuboid
+   * \param cuboid_pose - centroid of object to grasp in world frame
+   * \param depth length of cuboid along local x-axis
+   * \param width length of cuboid along local y-axis
+   * \param height length of cuboid along local z-axis
+   * \param grasp_data data describing end effector
+   * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types
+   * \param grasp_candidates possible grasps generated
+   * \return true if successful
+   */
+  bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                      const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) override;
+
+  bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                      const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates);
+
+  /**
+   * \brief Setter for grasp score weights
+   */
+  void setGraspScoreWeights(const TwoFingerGraspScoreWeightsPtr& grasp_score_weights)
+  {
+    auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(*grasp_score_weights);
+    grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights);
+  }
+
+  /**
+   * \brief Setter for grasp score weights
+   */
+  const TwoFingerGraspScoreWeightsPtr getGraspScoreWeights()
+  {
+    return std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_);
+  }
+
+protected:
+  /**
+   * \brief creates grasp messages from the generated grasp poses
+   * \param grasp_pose_eef_mount - the grasp pose. (Note: this is the pose of the eef mount not the position of the tcp)
+   * \param grasp_data data describing the end effector
+   * \param object_pose - pose of object to grasp
+   * \param object_size - size of object to grasp
+   * \param object_width - In the case of finger grippers, the width of the object in the dimension betwen the fingers
+   * \param grasp_candidates - list possible grasps with new grasp appended
+   * \return true on success
+   */
+  bool addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, const TwoFingerGraspDataPtr& grasp_data,
+                const Eigen::Isometry3d& object_pose, const Eigen::Vector3d& object_size, double object_width,
+                std::vector<GraspCandidatePtr>& grasp_candidates);
+
+  /**
+   * \brief Create grasp positions around one axis of a cuboid
+   * \param cuboid_pose:      centroid of object to grasp in world frame
+   * \param depth:            length of cuboid along local x-axis
+   * \param width:            length of cuboid along local y-axis
+   * \param height:           length of cuboid along local z-axis
+   * \param axis:             axis of cuboid to generate grasps along
+   * \param grasp_data:       data describing end effector
+   * \param grasp_candidates: possible grasps generated
+   * \param only_edge_grasps: set to true if object is too wide to grap the face in this axis
+   * \return true if successful
+   */
+  bool generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                                grasp_axis_t axis, const TwoFingerGraspDataPtr& grasp_data,
+                                const TwoFingerGraspCandidateConfig& grasp_candidate_config,
+                                std::vector<GraspCandidatePtr>& grasp_candidates);
+
+  /**
+   * \brief helper function for adding grasps at corner of cuboid
+   * \param pose - pose of the object to grasp
+   * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
+   * \param translation - translation to go from cuboid centroid to grasping location
+   * \param corner_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
+   * \param num_radial_grasps - the number of grasps to generate around the corner
+   * \param grasp_poses_tcp - list of grasp poses generated
+   * \return the number of poses generated
+   */
+  std::size_t addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
+                                    const Eigen::Vector3d& translation, double corner_rotation,
+                                    std::size_t num_radial_grasps, EigenSTL::vector_Isometry3d& grasp_poses_tcp);
+
+  /**
+   * \brief helper function for adding grasps along the face of a cuboid
+   * \param pose - pose of the object to grasp
+   * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
+   * \param delta - distance to move away from cuboid at each step
+   * \param translation - translation to go from cuboid centroid to grasping location
+   * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
+   * \param num_grasps - the number of grasps to generate around the corner
+   * \param grasp_poses_tcp - list of grasp poses generated
+   * \return the number of poses generated
+   */
+  std::size_t addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
+                                  const Eigen::Vector3d& translation, const Eigen::Vector3d& delta,
+                                  double alignment_rotation, std::size_t num_grasps,
+                                  EigenSTL::vector_Isometry3d& grasp_poses_tcp);
+
+  /**
+   * \brief helper function for adding grasps along the edges of the cuboid
+   * \param pose - pose of the object to grasp
+   * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
+   * \param delta - distance to move away from cuboid at each step
+   * \param translation - translation to go from cuboid centroid to grasping location
+   * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
+   * \param num_grasps - the number of grasps to generate around the corner
+   * \param grasp_poses_tcp - list of grasp poses generated
+   * \return the number of poses generated
+   */
+  std::size_t addEdgeGraspsHelper(const Eigen::Isometry3d& cuboid_pose, double rotation_angles[3],
+                                  const Eigen::Vector3d& translation, const Eigen::Vector3d& delta,
+                                  double alignment_rotation, std::size_t num_grasps,
+                                  EigenSTL::vector_Isometry3d& grasp_poses_tcp, double corner_rotation);
+
+  /**
+   * \brief helper function for determining if the grasp will intersect the cuboid
+   * \param cuboid_pose - centroid of object to grasp in world frame
+   * \param depth - size of cuboid along x axis
+   * \param width - size of cuboid along y axis
+   * \param height - size of cuboid along z axis
+   * \param grasp_pose_tcp - pose of grasp
+   * \param grasp_data - data describing end effector
+   * \return true if the grasp intersects the cuboid
+   */
+  bool graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                               const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data);
+
+  /**
+   * \brief helper function to test intersection of a line with a plane
+   * \param t - parametric distance along grasp line
+   * \param u1, v1, u2, v2 - (u,v) coordinates of the line
+   * \param a, b - length and width of the plane area in which to test for intersection
+   * \param u, v - location of intersection
+   * \return true if the line intersects the plane
+   */
+  bool intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, double b, double& u,
+                          double& v);
+
+  /**
+   * \brief Score the generated finger grasp poses
+   * \param grasp_pose_tcp - the grasp pose of the tcp
+   * \param grasp_data - data describing the end effector
+   * \param object_pose - the pose of the object being grasped
+   * \param percent_open - percentage that the grippers are open. 0.0 -> grippers are at object width + padding
+   * \return a score with positive being better
+   */
+  double scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data,
+                          const Eigen::Isometry3d& object_pose, double percent_open);
+  bool
+  generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
+                       const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates,
+                       const TwoFingerGraspCandidateConfig& grasp_candidate_config = TwoFingerGraspCandidateConfig());
+
+protected:
+  TwoFingerGraspCandidateConfig grasp_candidate_config_;
+
+  // Tests
+  FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateFaceGrasps);
+  FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateEdgeGrasps);
+  FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateCornerGrasps);
+
+};  // end of class
+
+typedef std::shared_ptr<TwoFingerGraspGenerator> TwoFingerGraspGeneratorPtr;
+typedef std::shared_ptr<const TwoFingerGraspGenerator> TwoFingerGraspGeneratorConstPtr;
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/include/two_finger_grasp_scorer.h b/src/mtc/include/two_finger_grasp_scorer.h
new file mode 100644
index 0000000000000000000000000000000000000000..629cc9d55696a13240b7ffa58ea92c3398597995
--- /dev/null
+++ b/src/mtc/include/two_finger_grasp_scorer.h
@@ -0,0 +1,104 @@
+/*********************************************************************
+ * Software License Agreement ("Modified BSD License")
+ *
+ * Copyright (c) 2014, University of Colorado, Boulder
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Univ of CO, Boulder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+/**
+ * Authors : Andy McEvoy
+ * Desc    : Functions for scoring generated grasps
+ */
+
+#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_
+#define MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_
+
+#include "../include/two_finger_grasp_data.h"
+#include "../include/grasp_scorer.h"
+
+namespace moveit_grasps
+{
+struct TwoFingerGraspScoreWeights : public GraspScoreWeights
+{
+  TwoFingerGraspScoreWeights() : GraspScoreWeights(), depth_score_weight_(1.0), width_score_weight_(1.0)
+  {
+  }
+
+  /* \brief Compute the weighted score given the orientation and translation scores */
+  double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores,
+                      double depth_score, double width_score, bool verbose = false) const;
+
+  /* \brief returns the sum of the grasp score weights*/
+  double getWeightTotal() const override;
+
+  // Finger gripper specific weights
+  double depth_score_weight_;
+  double width_score_weight_;
+};
+// Create smart pointers for this class
+typedef std::shared_ptr<TwoFingerGraspScoreWeights> TwoFingerGraspScoreWeightsPtr;
+
+class TwoFingerGraspScorer : public GraspScorer
+{
+public:
+  /**
+   * \brief Scores the grasp on how wide the fingers are on approach, the more open the better
+   * \param grasp_data - pointer to grasp info
+   * \param percent_open - amount the gripper is open
+   *                       0.0 -> gripper is open to the object width + minimum padding
+   *                       1.0 -> gripper is in full open position
+   * \return the unweighted score:
+   *         1.0 -> gripper is wide open,
+   *         0.0 -> gripper is at minimum position.
+   */
+  static double scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open);
+
+  /**
+   * \brief Score the grasp based on how far the object is from the palm of the hand
+   * \param grasp_pose_tcpgrasp_pose_tcp - the pose of the end effector (not the eef mount)
+   * \param grasp_data - pointer to grasp info
+   * \param object_pose - the pose of the object being grasped
+   * \param max_grasp_distance - the maximum acceptable distance from palm
+   * \return the unweighted score:
+   *         1.0 -> object pose and grasp pose have same translation values
+   *         0.0 -> object is at max distanct
+   *       < 0.0 -> object is beyond the max_grasp_distance
+   */
+  // DEV NOTE: when this function is called we've lost the references to the acutal size of the object.
+  // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object
+  // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an
+  // arbitrary number given our target object set (i.e. I based it off of the cheese it box)
+  static double scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data,
+                                    const Eigen::Isometry3d& object_pose, double min_grasp_distance,
+                                    double max_grasp_distance);
+};
+
+}  // namespace moveit_grasps
+
+#endif
diff --git a/src/mtc/launch/base_routine.launch b/src/mtc/launch/base_routine.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a021de08a8fb9410e8753334f40ecf5e16779687
--- /dev/null
+++ b/src/mtc/launch/base_routine.launch
@@ -0,0 +1,6 @@
+<launch>
+    <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
+
+    <node pkg="mtc" type="base_routine" name="base_routine" output="screen" required="true"/>
+</launch>
\ No newline at end of file
diff --git a/src/mtc/launch/gdb_settings.gdb b/src/mtc/launch/gdb_settings.gdb
new file mode 100644
index 0000000000000000000000000000000000000000..698fff8efb266732eff05bc9b707f16333ca24fc
--- /dev/null
+++ b/src/mtc/launch/gdb_settings.gdb
@@ -0,0 +1,8 @@
+## This file allows developers to set breakpoints in the CPP_EXECUTABLE_NAME.launch file
+## To set a breakpoint, compile with debug flag set to true. Add a line below with the source file name,
+## a colon, then the line number to break on. Then launch CPP_EXECUTABLE_NAME with argument debug:=true
+
+set breakpoint pending on
+
+## Example break point:
+# break moveit_visual_tools.cpp:322
diff --git a/src/mtc/launch/grasp_pipeline.launch b/src/mtc/launch/grasp_pipeline.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c5019dc44dd230118356336696ce87ee62016623
--- /dev/null
+++ b/src/mtc/launch/grasp_pipeline.launch
@@ -0,0 +1,49 @@
+<launch>
+  <!-- Launch rviz.launch before running this demo to see visalizations -->
+
+  <!-- Suction or two finger -->
+  <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" />
+
+  <!-- GDB Debug Arguments -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix"
+           value="gdb -x $(find mtc)/launch/gdb_settings.gdb --ex run --args" />
+
+  <!-- Verbose mode -->
+  <arg name="verbose" default="false" />
+  <arg unless="$(arg verbose)" name="command_args" value="" />
+  <arg     if="$(arg verbose)" name="command_args" value="--verbose" />
+
+  <!-- Callgrind Arguments -->
+  <arg name="callgrind" default="false" />
+  <arg unless="$(arg callgrind)" name="launch_prefix2" value="" />
+  <arg     if="$(arg callgrind)" name="launch_prefix2" value="valgrind --tool=callgrind --collect-atstart=no" />
+
+  <!-- Valgrind Arguments -->
+  <arg name="valgrind" default="false" />
+  <arg unless="$(arg valgrind)" name="launch_prefix3" value="" />
+  <arg     if="$(arg valgrind)" name="launch_prefix3" value="valgrind --tool=memcheck --leak-check=full --show-leak-kinds=all --track-origins=yes -v" />
+
+  <!-- PANDA -->
+  <include file="$(find mtc)/launch/load_panda.launch">
+    <arg name="gripper" value="$(arg gripper)"/>
+  </include>
+
+  <!-- Start the demo -->
+  <node name="moveit_grasps" launch-prefix="$(arg launch_prefix) $(arg launch_prefix2) $(arg launch_prefix3)" pkg="mtc"
+  type="moveit_grasps_grasp_pipeline" output="screen" args="$(arg command_args)">
+    <param name="gripper" value="$(arg gripper)"/>
+    <param name="ee_group_name" value="hand"/>
+    <param name="planning_group_name" value="panda_arm"/>
+    <rosparam command="load" file="$(find mtc)/config/panda_grasp_data.yaml"/>
+    <rosparam command="load" file="$(find mtc)/config/moveit_grasps_config.yaml"/>
+  </node>
+
+  <!-- Planning Functionality -->
+  <arg name="planner" default="ompl" />
+  <include ns="moveit_grasps" file="$(find panda_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="$(arg planner)" />
+  </include>
+
+</launch>
diff --git a/src/mtc/launch/load_panda.launch b/src/mtc/launch/load_panda.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2f2cac409a279f48a63f8d57cefccff35a638ae2
--- /dev/null
+++ b/src/mtc/launch/load_panda.launch
@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- Suction or two finger -->
+  <arg name="gripper" default="suction" doc="must be one of 'suction' or 'two_finger'" />
+
+  <!-- PANDA -->
+  <include file="$(find mtc)/launch/panda_planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+    <arg name="gripper" value="$(arg gripper)"/>
+  </include>
+
+  <!-- We do not have a robot connected, so publish fake joint states -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="false"/>
+    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  </node>
+  <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+</launch>
diff --git a/src/mtc/launch/mtc2taskspace.launch b/src/mtc/launch/mtc2taskspace.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c165f181ab10ac9af290c6f0d27a68518a921616
--- /dev/null
+++ b/src/mtc/launch/mtc2taskspace.launch
@@ -0,0 +1,4 @@
+<launch>
+    <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/>
+    <node pkg="mtc" type="mtc2taskspace" name="mtc2taskspace" output="screen" required="true"/>
+</launch>
\ No newline at end of file
diff --git a/src/mtc/launch/panda_planning_context.launch b/src/mtc/launch/panda_planning_context.launch
new file mode 100644
index 0000000000000000000000000000000000000000..bc394dca652f570c3087102af44d444142713c22
--- /dev/null
+++ b/src/mtc/launch/panda_planning_context.launch
@@ -0,0 +1,31 @@
+<launch>
+  <!-- Suction or two finger -->
+  <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" />
+
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="false"/>
+
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
+
+  <!-- Should the panda be loaded with the two finger gripper or the suction gripper -->
+
+  <!-- Load universal robot description format (URDF) -->
+  <param if="$(eval arg('load_robot_description') and arg('gripper')=='two_finger')" name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_two_finger_hand.urdf.xacro'"/>
+  <param if="$(eval arg('load_robot_description') and arg('gripper')=='suction')"    name="$(arg robot_description)" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_suction_hand.urdf.xacro'"/>
+
+  <!-- The semantic description that corresponds to the URDF -->
+  <param if="$(eval arg('gripper')=='two_finger')" name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_two_finger_hand.srdf.xacro'"  />
+  <param if="$(eval arg('gripper')=='suction')"    name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find moveit_grasps)/robots/panda_arm_suction_hand.srdf.xacro'"     />
+
+  <!-- Load updated joint limits (override information from URDF) -->
+  <group ns="$(arg robot_description)_planning">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml"/>
+  </group>
+
+  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+  <group ns="$(arg robot_description)_kinematics">
+    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+  </group>
+
+</launch>
diff --git a/src/mtc/launch/rviz.launch b/src/mtc/launch/rviz.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7d8dcc957904118ab3e96aef3c8c81d3651068f9
--- /dev/null
+++ b/src/mtc/launch/rviz.launch
@@ -0,0 +1,25 @@
+<launch>
+  <!-- Debug -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <!-- Suction or two finger -->
+  <arg name="gripper" default="two_finger" doc="must be one of 'suction' or 'two_finger'" />
+
+  <!-- Publish world frame -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
+
+  <!-- PANDA -->
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+    <arg name="GOAL" value="0,0,0"/>
+
+  </include>
+
+  <!-- Load Rviz -->
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+	args="-d $(find mtc)/launch/rviz/grasps.rviz" output="screen">
+  </node>
+
+</launch>
diff --git a/src/mtc/launch/rviz/grasps.rviz b/src/mtc/launch/rviz/grasps.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..be0d012ee8d190bb8560f47c27646da12163c621
--- /dev/null
+++ b/src/mtc/launch/rviz/grasps.rviz
@@ -0,0 +1,386 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MarkerArray1/Namespaces1
+      Splitter Ratio: 0.5704230070114136
+    Tree Height: 545
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+  - Class: rviz_visual_tools/RvizVisualToolsGui
+    Name: RvizVisualToolsGui
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/Trajectory
+      Color Enabled: false
+      Enabled: true
+      Interrupt Display: false
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_center_point:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Loop Animation: false
+      Name: Trajectory
+      Robot Alpha: 0.5
+      Robot Color: 150; 50; 150
+      Robot Description: robot_description
+      Show Robot Collision: false
+      Show Robot Visual: true
+      Show Trail: false
+      State Display Time: 0.05 s
+      Trail Step Size: 1
+      Trajectory Topic: /display_planned_path
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /grasp_visuals
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Attached Body Color: 150; 50; 150
+      Class: moveit_rviz_plugin/RobotState
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tool_center_point:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotState
+      Robot Alpha: 1
+      Robot Description: robot_description
+      Robot State Topic: /display_robot_state
+      Show All Links: true
+      Show Highlights: true
+      Value: true
+      Visual Enabled: true
+    - Class: moveit_rviz_plugin/PlanningScene
+      Enabled: true
+      Move Group Namespace: ""
+      Name: PlanningScene
+      Planning Scene Topic: /moveit_grasps_demo/grasping_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 0.8999999761581421
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.20000000298023224
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          panda_hand:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_leftfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link7:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          panda_link8:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          panda_rightfinger:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          tool_center_point:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 1
+        Show Robot Collision: false
+        Show Robot Visual: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+    - Class: rviz_visual_tools/KeyTool
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.91066575050354
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.6259773373603821
+        Y: 0.3534509539604187
+        Z: 0.4344802796840668
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.14539791643619537
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.575382709503174
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 845
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000164000002acfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000042c000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ac000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000220000001c00000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100ffffff000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003f1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005fd00000041fc0100000003fb00000024005200760069007a00560069007300750061006c0054006f006f006c00730047007500690100000000000005fd0000016400fffffffb0000000800540069006d0065000000000000000638000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000493000002ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  RvizVisualToolsGui:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Trajectory - Trajectory Slider:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1533
+  X: 67
+  Y: 27
diff --git a/src/mtc/maps/dummy.yaml b/src/mtc/maps/dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..85ae62136502cfe89287c6002d65cd821866edab
--- /dev/null
+++ b/src/mtc/maps/dummy.yaml
@@ -0,0 +1,44 @@
+data:
+  - -0.704647 -0.085013 0.103821 -0.063726 -0.039416 -0.249854 0.965380
+  - -0.513205 -0.514270 0.103184 0.092404 -0.010381 -0.590406 0.801732
+  - -0.499355 -0.290624 0.104579 -0.041985 -0.015622 -0.411819 0.910164
+  - -0.459296 -0.182886 0.049981 -0.221514 0.461441 -0.859037 0.007668
+  - -0.519670 0.093757 0.102952 0.057347 -0.080399 -0.305704 0.946991
+  - -0.499105 0.404992 0.000930 -0.557591 0.429723 0.428737 0.566228
+  - -0.489862 0.521042 0.102369 -0.020562 0.110022 0.800758 0.588437
+  - -0.294706 -0.580002 0.067799 0.390187 -0.157800 0.394885 0.816651
+  - -0.322003 -0.302647 0.102635 0.061764 -0.086307 -0.477962 0.871946
+  - -0.297281 -0.032005 0.079964 -0.311869 -0.148179 -0.436303 0.830916
+  - -0.323814 0.102339 0.102237 0.017354 -0.113384 -0.245576 0.962567
+  - -0.221989 0.339610 0.058054 -0.434513 -0.186412 -0.879328 0.056848
+  - -0.282236 0.566463 -0.079323 -0.842237 0.410323 0.066782 0.343238
+  - -0.045863 -0.717152 0.088317 -0.256995 0.115736 -0.952974 0.111348
+  - -0.017530 -0.503598 0.064890 -0.243688 0.362787 -0.533595 0.724071
+  - -0.087573 -0.290814 0.103857 -0.066561 0.031855 -0.467458 0.880930
+  - -0.162281 0.379039 0.029983 -0.367444 -0.471388 -0.191603 0.778503
+  - -0.001871 0.499820 0.037360 -0.225484 0.520819 -0.328508 0.754975
+  - 0.000220 0.731249 -0.002125 0.004754 0.714210 0.212787 0.666785
+  - 0.097747 -0.713925 0.104048 0.058142 0.033945 -0.634190 0.770241
+  - 0.000591 -0.480898 -0.027892 -0.540596 -0.583588 0.320502 0.514257
+  - 0.177063 -0.326099 0.066371 -0.179291 0.389617 -0.620917 0.656137
+  - 0.071622 -0.165081 0.077357 0.042348 -0.360331 0.804873 0.469626
+  - 0.175867 0.259083 -0.059958 -0.622893 0.630490 -0.442872 0.135467
+  - 0.071068 0.437589 0.079327 0.239442 0.254795 -0.889244 0.294944
+  - 0.252831 -0.566984 0.065675 0.121811 -0.415237 0.561186 0.705557
+  - 0.295272 -0.292902 0.104653 0.015052 -0.037756 -0.977591 0.206554
+  - 0.284065 -0.076036 0.100979 -0.071881 -0.118235 -0.419818 0.896999
+  - 0.382397 0.139887 0.051428 -0.304978 0.402606 -0.169316 0.846303
+  - 0.238413 0.384767 0.006826 -0.655008 -0.196111 0.241573 0.688584
+  - 0.237968 0.455913 0.072342 0.158007 -0.361317 0.187635 0.899598
+  - 0.582562 -0.238750 0.021376 -0.629042 0.050133 -0.584335 0.510240
+  - 0.515243 -0.097356 0.103854 -0.073570 0.006677 -0.963172 0.258540
+  - 0.548987 0.125685 0.089250 -0.243039 0.126222 -0.550080 0.788930
+  - 0.526708 0.200380 -0.019687 0.363156 -0.679607 0.620766 0.144572
+  - 0.545807 0.477673 0.091806 -0.188434 0.165296 -0.933886 0.255002
+  - 0.698731 -0.108383 0.104657 0.037677 0.014594 -0.496260 0.867233
+  - 0.688405 0.103068 0.104313 0.013921 -0.055488 -0.482543 0.874002
+description:
+  system: Franka Emika 'Panda'
+  resolution: 0.2
+  dimension: 0.900000 0.900000 0.000000 (m)
+  timestamp: 1712.052411 sec
\ No newline at end of file
diff --git a/src/mtc/package.xml b/src/mtc/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1213557c52d3af5afd9af5b3363c849eb40c63af
--- /dev/null
+++ b/src/mtc/package.xml
@@ -0,0 +1,54 @@
+<package>
+  <name>mtc</name>
+  <version>2.0.0</version>
+  <description>
+    A basic grasp generator for objects such as blocks or cylinders for use with the MoveIt! pick and place pipeline.
+    Does not consider friction cones or other dynamics.
+  </description>
+  <maintainer email="davetcoleman@gmail.com">Dave Coleman</maintainer>
+  <maintainer email="mcevoy.andy@gmail.com">Andy McEvoy</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/PickNikRobotics/moveit_grasps/</url>
+  <url type="bugtracker">https://github.com/PickNikRobotics/moveit_grasps/issues</url>
+  <url type="repository">https://github.com/PickNikRobotics/moveit_grasps</url>
+
+  <author email="davetcoleman@gmail.com">Dave Coleman</author>
+  <author email="mcevoy.andy@gmail.com">Andy McEvoy</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslint</build_depend>
+  <build_depend>eigen_conversions</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>trajectory_msgs</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf_conversions</build_depend>
+  <build_depend>moveit_ros_planning</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_visual_tools</build_depend>
+  <build_depend>rosparam_shortcuts</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>YAML_CPP</build_depend> 
+
+  <run_depend>YAML_CPP</run_depend> 
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>trajectory_msgs</run_depend>
+  <run_depend>moveit_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>moveit_visual_tools</run_depend>
+  <run_depend>rosparam_shortcuts</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/src/mtc/resources/dummy.yaml b/src/mtc/resources/dummy.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0cfc174df5feb31252e335ae3167d2c08333979a
--- /dev/null
+++ b/src/mtc/resources/dummy.yaml
@@ -0,0 +1,140 @@
+# create json file using
+# yq eval -o=json config_scene_ceti-table-placeworld.yaml > config_scene_ceti-table-placeworld.json
+{ 'objects': [
+  # table
+  # height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
+  # wheels width/depth/height 12*12*12
+  # body width/depth/height 69*69*76
+  # table-top width/depth/height 80*80*1
+  # panels are 50*70, with 0.5 cm distance to the table-top
+  # height of both is 12 + 76 + .5 = 88.5
+  # their centre is at 40 + 0.25 + 25 = 65.25
+  # a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot
+  #  { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_wheel_1',     'pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_wheel_2',     'pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_wheel_3',     'pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_wheel_4',     'pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_front',  'pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_back',   'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_left',   'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_body_right',  'pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_table_top',   'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_left_panel',  'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table1_front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+  { 'id': 'shared_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+  # table 2 is shifted in y direction by the size of the base, the size of a side panel and two 5mm spaces:
+  # 0.8 + 0.5 + 2*0.05 = 1.31
+
+  { 'id': 'table2_wheel_1',     'pos': { 'x': 0.28,'y': 1.59,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_wheel_2',     'pos': { 'x': -0.28,'y': 1.03,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_wheel_3',     'pos': { 'x': -0.28,'y': 1.59,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_wheel_4',     'pos': { 'x': 0.28,'y': 1.03,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_body_front',  'pos': { 'x': .345, y: 1.31, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_body_back',   'pos': { 'x': -.345, y: 1.31, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_body_left',   'pos': { 'y': 1.655, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_body_right',  'pos': { 'y': 0.965, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_table_top',   'pos': { 'y': 1.31, 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_front_panel', 'pos': { 'x': 0.6525, 'y': 1.31, 'z': 0.885  },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+  { 'id': 'table2_right_panel', 'pos': { 'z': 0.885, 'y': 1.9625 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
+
+
+  { 'id': 'A1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'A4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'B1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'B2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'B3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'B4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'C1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'C4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'C7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'C9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y': -.1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'D7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .1, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .2, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'D9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  .3, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'E1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'E4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'F1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'F2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'F3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .6, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'F4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  .7, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'G1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'G4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'G7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.11, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'G9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.21, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H5', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H6', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'H7', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.41, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H8', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.51, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'H9', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .3, 'y':  1.61, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  { 'id': 'I1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.3, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'I4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x': -.2, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+
+  { 'id': 'J1', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+  { 'id': 'J2', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .1, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'J3', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  1.91, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+#  { 'id': 'J4', 'type': 'DROP_OFF_LOCATION', 'pos': { 'x':  .2, 'y':  2.01, 'z': 0.89 },'size': { 'length': 0.1,'width': 0.1,'height': 0.0 },'orientation': { 'w': 1 } },
+
+  # the collaboration zone has height 120cm
+  { 'id': 'cz1', 'type': 'COLLABORATION_ZONE', 'pos': { 'z': 1.49, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.3,'height': 1.2 },'orientation': { 'w': 1 },'color': { 'r': 1,'g': 0.627,'b': 0 } },
+
+  { 'id': 'blue1',  'type': 'BOX', 'pos': { 'x':  .1, 'y': -.7, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+  { 'id': 'blue2',  'type': 'BOX', 'pos': { 'x':  .2, 'y':  .3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # target B
+  { 'id': 'blue3',  'type': 'BOX', 'pos': { 'x':  .2, 'y': -.1, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } }, # impossible for the target B
+  { 'id': 'green1', 'type': 'BOX', 'pos': { 'x':  .2, 'y': -.3, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'green2', 'type': 'BOX', 'pos': { 'x':  .1, 'y':  1.91, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
+  { 'id': 'red1',   'type': 'BOX', 'pos': { 'x': -.3, 'y': -.6, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+  { 'id': 'red2',   'type': 'BOX', 'pos': { 'x':  .3, 'y':  1.41, 'z': 0.935500 },'size': { 'length': .0318, 'width': .0636, 'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
+
+  { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.22,            'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } },
+  { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.22, 'y': 1.31, 'z': 0.89 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
+] }
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c3a99d8b10c0554f0c74b2569007afcd801801c3
--- /dev/null
+++ b/src/mtc/src/base_routine.cpp
@@ -0,0 +1,31 @@
+#include "impl/abstract_map_loader.h"
+#include "impl/map_loader.h"
+#include "impl/abstract_strategy.h"
+#include "impl/base_by_rotation.h"
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "impl/map_loader.cpp"
+#include "impl/base_by_rotation.cpp"
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "base_routine");
+    ros::NodeHandle n;
+
+    XmlRpc::XmlRpcValue map;
+    XmlRpc::XmlRpcValue task;
+
+    n.getParam("/data",map);
+    n.getParam("/task/groups",task);
+
+    Abstract_map_loader* map_loader = new Map_loader(map, task);
+    Abstract_strategy* strategy = new Base_by_rotation();
+
+    map_loader->set_strategy(strategy);
+
+    map_loader->base_calculation();
+
+    free(map_loader);
+    free(strategy);
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/mtc/src/grasp_candidate.cpp b/src/mtc/src/grasp_candidate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5482843520f118f662762f1e95c05e1afd0c0f2d
--- /dev/null
+++ b/src/mtc/src/grasp_candidate.cpp
@@ -0,0 +1,116 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Filters grasps based on kinematic feasibility and collision
+*/
+
+#include "../include/grasp_candidate.h"
+
+namespace moveit_grasps
+{
+GraspCandidate::GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data,
+                               const Eigen::Isometry3d& cuboid_pose)
+  : grasp_(grasp)
+  , grasp_data_(grasp_data)
+  , cuboid_pose_(cuboid_pose)
+  , grasp_filtered_code_(GraspFilterCode::NOT_FILTERED)
+{
+}
+
+bool GraspCandidate::getPreGraspState(moveit::core::RobotStatePtr& robot_state)
+{
+  // Error check
+  if (pregrasp_ik_solution_.empty())
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_candidate", "No pregrasp ik solution available to set");
+    return false;
+  }
+
+  // Apply IK solved arm joints to state
+  robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, pregrasp_ik_solution_);
+
+  // Set end effector to correct configuration
+  grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture);
+
+  return true;
+}
+
+bool GraspCandidate::getGraspStateOpen(moveit::core::RobotStatePtr& robot_state)
+{
+  // Error check
+  if (grasp_ik_solution_.empty())
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set");
+    return false;
+  }
+
+  // Apply IK solved arm joints to state
+  robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_);
+
+  // Set end effector to correct configuration
+  return getGraspStateOpenEEOnly(robot_state);
+}
+
+bool GraspCandidate::getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state)
+{
+  return grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture);
+}
+
+bool GraspCandidate::getGraspStateClosed(moveit::core::RobotStatePtr& robot_state)
+{
+  if (grasp_ik_solution_.empty())
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set");
+    return false;
+  }
+
+  // Apply IK solved arm joints to state
+  robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_);
+
+  // Set end effector to correct configuration
+  return getGraspStateClosedEEOnly(robot_state);
+}
+
+bool GraspCandidate::getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state)
+{
+  return grasp_data_->setRobotState(robot_state, grasp_.grasp_posture);
+}
+
+bool GraspCandidate::isValid()
+{
+  return grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/grasp_data.cpp b/src/mtc/src/grasp_data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69114492a8726ce11da01ee908289a331e6c654
--- /dev/null
+++ b/src/mtc/src/grasp_data.cpp
@@ -0,0 +1,212 @@
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L.
+ *     nor the names of its contributors may be used to endorse or
+ *     promote products derived from this software without specific
+ *     prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Authors: Dave Coleman, Bence Magyar
+   Description: Data class used by the grasp generator.
+*/
+
+#include "../include/grasp_data.h"
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <eigen_conversions/eigen_msg.h>
+
+// C++
+#include <cmath>
+
+// Parameter loading
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+// Pose conversion
+#include <rviz_visual_tools/rviz_visual_tools.h>
+
+namespace moveit_grasps
+{
+const std::string LOGNAME = "grasp_data";
+
+GraspData::GraspData(const ros::NodeHandle& nh, const std::string& end_effector,
+                     const moveit::core::RobotModelConstPtr& robot_model)
+  : base_link_("/base_link"), robot_model_(robot_model)
+{
+}
+
+bool GraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector)
+{
+  std::vector<std::string> joint_names;
+  std::vector<double> pre_grasp_posture;  // todo: remove all underscore post-fixes
+  std::vector<double> grasp_posture;
+  double pregrasp_time_from_start;
+  double grasp_time_from_start;
+  std::string end_effector_name;
+
+  // Helper to let user know what is wrong
+  if (!nh.hasParam("base_link"))
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_data", "Grasp configuration parameter `base_link` missing from rosparam "
+                                         "server. Did you load your end effector's configuration yaml file? "
+                                         "Searching in namespace: "
+                                             << nh.getNamespace());
+    return false;
+  }
+
+  // Load all other parameters
+  const std::string parent_name = "grasp_data";  // for namespacing logging messages
+  std::size_t error = 0;
+  error += !rosparam_shortcuts::get(parent_name, nh, "base_link", base_link_);
+
+  // Search within the sub-namespace of this end effector name
+  ros::NodeHandle child_nh(nh, end_effector);
+
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_time_from_start", pregrasp_time_from_start);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_time_from_start", grasp_time_from_start);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_resolution", grasp_resolution_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_min_depth", grasp_min_depth_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_max_depth", grasp_max_depth_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_depth_resolution", grasp_depth_resolution_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "approach_distance_desired", approach_distance_desired_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "retreat_distance_desired", retreat_distance_desired_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "lift_distance_desired", lift_distance_desired_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "angle_resolution", angle_resolution_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "end_effector_name", end_effector_name);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "joints", joint_names);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_padding_on_approach", grasp_padding_on_approach_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_posture", grasp_posture);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_posture", pre_grasp_posture);
+
+  bool define_tcp_by_name;
+  child_nh.param<bool>("define_tcp_by_name", define_tcp_by_name, false);
+  if (define_tcp_by_name)
+    error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_name", tcp_name_);
+  else
+    error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_to_eef_mount_transform", tcp_to_eef_mount_);
+
+  rosparam_shortcuts::shutdownIfError(parent_name, error);
+
+  // Create pre-grasp and grasp postures
+  pre_grasp_posture_.header.frame_id = base_link_;
+  pre_grasp_posture_.header.stamp = ros::Time::now();
+  // Name of joints:
+  pre_grasp_posture_.joint_names = joint_names;
+  // Position of joints
+  pre_grasp_posture_.points.resize(1);
+  pre_grasp_posture_.points[0].positions = pre_grasp_posture;
+  pre_grasp_posture_.points[0].time_from_start = ros::Duration(pregrasp_time_from_start);
+
+  // Create grasp posture
+  grasp_posture_.header.frame_id = base_link_;
+  grasp_posture_.header.stamp = ros::Time::now();
+  // Name of joints:
+  grasp_posture_.joint_names = joint_names;
+  // Position of joints
+  grasp_posture_.points.resize(1);
+  grasp_posture_.points[0].positions = grasp_posture;
+  grasp_posture_.points[0].time_from_start = ros::Duration(grasp_time_from_start);
+
+  // Copy values from RobotModel
+  ee_jmg_ = robot_model_->getJointModelGroup(end_effector_name);
+  arm_jmg_ = robot_model_->getJointModelGroup(ee_jmg_->getEndEffectorParentGroup().first);
+  parent_link_ = robot_model_->getLinkModel(ee_jmg_->getEndEffectorParentGroup().second);
+
+  ROS_INFO_NAMED("grasp_data", "ee_name: %s, arm_jmg: %s, parent_link: %s", ee_jmg_->getName().c_str(),
+                 arm_jmg_->getName().c_str(), parent_link_->getName().c_str());
+
+  if (define_tcp_by_name)
+  {
+    robot_state::RobotState state(robot_model_);
+    state.setToDefaultValues();
+    state.update();
+    if (!state.knowsFrameTransform(parent_link_->getName()))
+    {
+      ROS_ERROR_NAMED("grasp_data",
+                      "Robot Model does not know the frame transform for the end effector group parent "
+                      "frame: %s. Did you set a parent link in the srdf?",
+                      parent_link_->getName().c_str());
+    }
+    if (!state.knowsFrameTransform(tcp_name_))
+    {
+      ROS_ERROR_NAMED("grasp_data",
+                      "Robot Model does not know the frame transform for the tcp frame: %s. Is it "
+                      "available in the urdf?",
+                      tcp_name_.c_str());
+    }
+    Eigen::Isometry3d eef_mount_pose = state.getGlobalLinkTransform(parent_link_);
+    Eigen::Isometry3d tcp_mount_pose = state.getGlobalLinkTransform(tcp_name_);
+    tcp_to_eef_mount_ = tcp_mount_pose.inverse() * eef_mount_pose;
+  }
+
+  return true;
+}
+
+bool GraspData::setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state)
+{
+  ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStatePreGrasp is probably wrong");
+  return setRobotState(robot_state, pre_grasp_posture_);
+}
+
+bool GraspData::setRobotStateGrasp(robot_state::RobotStatePtr& robot_state)
+{
+  ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStateGrasp is probably wrong");
+  return setRobotState(robot_state, grasp_posture_);
+}
+
+bool GraspData::setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture)
+{
+  // TODO(davetcoleman): make this more efficient
+  // Do for every joint in end effector
+  for (std::size_t i = 0; i < posture.joint_names.size(); ++i)
+  {
+    // Set joint position
+    robot_state->setJointPositions(posture.joint_names[i], posture.points[0].positions);
+  }
+  return true;
+}
+
+void GraspData::print()
+{
+  ROS_WARN_STREAM_NAMED("grasp_data", "Debug Grasp Data variable values:");
+  std::cout << "tcp_to_eef_mount_: \n"
+            << tcp_to_eef_mount_.translation() << "\n"
+            << tcp_to_eef_mount_.rotation() << std::endl;
+  std::cout << "pre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl;
+  std::cout << "grasp_posture_: \n" << grasp_posture_ << std::endl;
+  std::cout << "base_link_: " << base_link_ << std::endl;
+  std::cout << "ee_group_: " << ee_jmg_->getName() << std::endl;
+  std::cout << "angle_resolution_: " << angle_resolution_ << std::endl;
+  std::cout << "grasp_max_depth_: " << grasp_max_depth_ << std::endl;
+  std::cout << "grasp_padding_on_approach_: " << grasp_padding_on_approach_ << std::endl;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/grasp_filter.cpp b/src/mtc/src/grasp_filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fd7ec8f9e4d9fec29605cafcbc4640e5fb1061c2
--- /dev/null
+++ b/src/mtc/src/grasp_filter.cpp
@@ -0,0 +1,884 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Filters grasps based on kinematic feasibility and collision
+*/
+
+// moveit_grasps
+#include "../include/grasp_filter.h"
+#include "../include/state_validity_callback.h"
+
+// moveit
+#include <moveit/transforms/transforms.h>
+#include <moveit/collision_detection/collision_tools.h>
+
+// Conversions
+#include <eigen_conversions/eigen_msg.h>
+
+// Parameter loading
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+namespace moveit_grasps
+{
+// Constructor
+GraspFilter::GraspFilter(const robot_state::RobotStatePtr& robot_state,
+                         const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools)
+  : name_("grasp_filter"), visual_tools_(visual_tools), nh_("~/moveit_grasps/filter")
+{
+  // Make a copy of the robot state so that we are sure outside influence does not break our grasp filter
+  robot_state_ = std::make_shared<moveit::core::RobotState>(*robot_state);
+  robot_state_->update();  // make sure transforms are computed
+
+  // Load visulization settings
+  const std::string parent_name = "grasp_filter";  // for namespacing logging messages
+  std::size_t error = 0;
+  // clang-format off
+  error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose", collision_verbose_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "statistics_verbose", statistics_verbose_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose_speed", collision_verbose_speed_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_grasps", show_filtered_grasps_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions", show_filtered_arm_solutions_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_cutting_planes", show_cutting_planes_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_speed", show_filtered_arm_solutions_speed_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_pregrasp_speed", show_filtered_arm_solutions_pregrasp_speed_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_grasp_filter_collision_if_failed", show_grasp_filter_collision_if_failed_);
+  // clang-format on
+
+  rosparam_shortcuts::shutdownIfError(parent_name, error);
+
+  // Add planning scene publisher for debugging
+  planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("/published_planning_scene", 100, false);
+}
+
+bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
+                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                               const robot_model::JointModelGroup* arm_jmg,
+                               const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
+                               const std::string& target_object_id)
+{
+  planning_scene::PlanningScenePtr planning_scene;
+  {
+    planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor);
+    planning_scene = planning_scene::PlanningScene::clone(scene);
+  }
+  return filterGrasps(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, target_object_id);
+}
+
+bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
+                               const planning_scene::PlanningScenePtr& planning_scene,
+                               const robot_model::JointModelGroup* arm_jmg,
+                               const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
+                               const std::string& target_object_id)
+{
+  bool verbose = false;
+
+  // Error check
+  if (grasp_candidates.empty())
+  {
+    ROS_ERROR_NAMED(name_, "Unable to filter grasps because vector is empty");
+    return false;
+  }
+  if (!filter_pregrasp)
+    ROS_WARN_STREAM_NAMED(name_, "Not filtering pre-grasp - GraspCandidate may have bad data");
+
+  // Visualize the cutting planes if desired
+  visualizeCuttingPlanes();
+
+  // Get the solver timeout from kinematics.yaml
+  solver_timeout_ = arm_jmg->getDefaultIKTimeout();
+  ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Grasp filter IK timeout " << solver_timeout_);
+
+  // Choose how many degrees of freedom
+  num_variables_ = arm_jmg->getVariableCount();
+  ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Solver for " << num_variables_ << " degrees of freedom");
+
+  // Get the end effector joint model group
+  if (arm_jmg->getAttachedEndEffectorNames().size() == 0)
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "No end effectors attached to this arm");
+    return false;
+  }
+  else if (arm_jmg->getAttachedEndEffectorNames().size() > 1)
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "More than one end effectors attached to this arm");
+    return false;
+  }
+
+  // Try to filter grasps not in verbose mode
+  std::size_t remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state,
+                                                    filter_pregrasp, verbose, target_object_id);
+
+  // Print stats
+  printFilterStatistics(grasp_candidates);
+
+  if (remaining_grasps == 0)
+  {
+    ROS_INFO_STREAM_NAMED(name_, "Grasp filters removed all grasps");
+    if (show_grasp_filter_collision_if_failed_)
+    {
+      ROS_INFO_STREAM_NAMED(name_, "Re-running in verbose mode since it failed");
+      verbose = true;
+      remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp,
+                                            verbose, target_object_id);
+    }
+    else
+      ROS_INFO_STREAM_NAMED(name_, "NOT re-running in verbose mode");
+  }
+
+  // Visualize valid grasps as arrows with cartesian path as well
+  if (show_filtered_grasps_)
+  {
+    ROS_INFO_STREAM_NAMED(name_, "Showing filtered grasps");
+    visualizeGrasps(grasp_candidates, arm_jmg);
+  }
+
+  // Visualize valid grasp as arm positions
+  if (show_filtered_arm_solutions_)
+  {
+    ROS_INFO_STREAM_NAMED(name_, "Showing filtered arm solutions");
+    visualizeCandidateGrasps(grasp_candidates);
+  }
+
+  if (grasp_candidates.empty())
+  {
+    ROS_WARN_STREAM_NAMED(name_, "No grasps remaining after filtering");
+    return false;
+  }
+
+  return true;
+}
+
+bool GraspFilter::filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose,
+                                     GraspParallelPlane plane, int direction) const
+{
+  Eigen::Isometry3d grasp_pose;
+  Eigen::Vector3d grasp_position;
+
+  // get grasp translation in filter pose CS
+  grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose);
+  grasp_position = filter_pose.inverse() * grasp_pose.translation();
+
+  // filter grasps by cutting plane
+  double epsilon = 0.00000001;
+  switch (plane)
+  {
+    case XY:
+      if ((direction == -1 && grasp_position(2) < 0 + epsilon) || (direction == 1 && grasp_position(2) > 0 - epsilon))
+        grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE;
+      break;
+    case XZ:
+      if ((direction == -1 && grasp_position(1) < 0 + epsilon) || (direction == 1 && grasp_position(1) > 0 - epsilon))
+        grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE;
+      break;
+    case YZ:
+      if ((direction == -1 && grasp_position(0) < 0 + epsilon) || (direction == 1 && grasp_position(0) > 0 - epsilon))
+        grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE;
+      break;
+    default:
+      ROS_WARN_STREAM_NAMED("filter_by_plane", "plane not specified correctly");
+      break;
+  }
+
+  return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE;
+}
+
+bool GraspFilter::filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose,
+                                           double max_angular_offset) const
+{
+  Eigen::Isometry3d tcp_grasp_pose;
+  Eigen::Isometry3d eef_mount_grasp_pose;
+  Eigen::Vector3d desired_z_axis;
+  Eigen::Vector3d grasp_z_axis;
+  double angle;
+
+  // convert grasp pose back to standard grasping orientation
+  eef_mount_grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose);
+  tcp_grasp_pose = eef_mount_grasp_pose * grasp_candidate->grasp_data_->tcp_to_eef_mount_.inverse();
+
+  // compute the angle between the z-axes of the desired and grasp poses
+  grasp_z_axis = tcp_grasp_pose.rotation() * Eigen::Vector3d(0, 0, 1);
+  desired_z_axis = desired_pose.rotation() * Eigen::Vector3d(0, 0, 1);
+  double cos_angle = grasp_z_axis.normalized().dot(desired_z_axis.normalized());
+  angle = acos(std::max(-1.0, std::min(1.0, cos_angle)));
+
+  if (angle > max_angular_offset)
+  {
+    ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "grasp filtered by orientation");
+    grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION;
+    return true;
+  }
+  else
+    return false;
+}
+
+bool GraspFilter::filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution,
+                                       const IkThreadStructPtr& ik_thread_struct) const
+{
+  // Get pose
+  ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose;
+
+  // Create constraint_fn
+  moveit::core::GroupStateValidityCallbackFn constraint_fn =
+      boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(),
+                  collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1,
+                  _2, _3);
+
+  // Set gripper position (eg. how open the eef is) to the custom open position
+  grasp_candidate->getGraspStateOpenEEOnly(ik_thread_struct->robot_state_);
+
+  // Solve IK Problem for grasp posture
+  grasp_ik_solution.resize(0);
+  if (!findIKSolution(grasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn))
+  {
+    ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find the-grasp IK solution");
+    grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK;
+  }
+  return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK;
+}
+
+bool GraspFilter::filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate,
+                                          std::vector<double>& pregrasp_ik_solution,
+                                          const IkThreadStructPtr& ik_thread_struct) const
+{
+  if (!ik_thread_struct->filter_pregrasp_)
+  {
+    ROS_DEBUG_STREAM_NAMED(name_, "Not filtering pregrasp");
+    return true;
+  }
+
+  // Set IK target pose to the pre-grasp pose
+  const std::string& ee_parent_link_name = grasp_candidate->grasp_data_->ee_jmg_->getEndEffectorParentGroup().second;
+  ik_thread_struct->ik_pose_ = GraspGenerator::getPreGraspPose(grasp_candidate, ee_parent_link_name);
+
+  moveit::core::GroupStateValidityCallbackFn constraint_fn =
+      boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(),
+                  collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1,
+                  _2, _3);
+
+  // Solve IK Problem for pregrasp
+  pregrasp_ik_solution.resize(0);
+  if (!findIKSolution(pregrasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn))
+  {
+    ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find PRE-grasp IK solution");
+    grasp_candidate->grasp_filtered_code_ = GraspFilterCode::PREGRASP_FILTERED_BY_IK;
+  }
+  return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK;
+}
+
+std::size_t
+GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                const robot_model::JointModelGroup* arm_jmg,
+                                const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, bool visualize,
+                                const std::string& target_object_id)
+{
+  planning_scene::PlanningScenePtr planning_scene;
+  {
+    planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor);
+    planning_scene = planning_scene::PlanningScene::clone(scene);
+  }
+  return filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, visualize,
+                            target_object_id);
+}
+
+std::size_t GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                            const planning_scene::PlanningScenePtr& planning_scene,
+                                            const robot_model::JointModelGroup* arm_jmg,
+                                            const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
+                                            bool visualize, const std::string& target_object_id)
+{
+  // Clone the planning scene for const correctness
+  planning_scene::PlanningScenePtr planning_scene_clone = planning_scene::PlanningScene::clone(planning_scene);
+
+  // Setup collision checking
+  *robot_state_ = planning_scene_clone->getCurrentState();
+
+  // Check that we have grasp candidates
+  if (!grasp_candidates.size())
+  {
+    ROS_WARN_NAMED(name_, "filterGraspsHelper passed empty grasp candidates");
+    return 0;
+  }
+
+  // Choose Number of cores
+  std::size_t num_threads = omp_get_max_threads();
+  if (num_threads > grasp_candidates.size())
+  {
+    num_threads = grasp_candidates.size();
+  }
+
+  // Debug
+  if (visualize || collision_verbose_)
+  {
+    num_threads = 1;
+    ROS_WARN_STREAM_NAMED(name_, "Using only " << num_threads << " threads because verbose is true");
+  }
+  ROS_INFO_STREAM_NAMED(name_, "Filtering " << grasp_candidates.size() << " candidate grasps with " << num_threads
+                                            << " threads");
+
+  // Load kinematic solvers if not already loaded
+  if (kin_solvers_[arm_jmg->getName()].size() != num_threads)
+  {
+    kin_solvers_[arm_jmg->getName()].clear();
+
+    // Create an ik solver for every thread
+    for (std::size_t i = 0; i < num_threads; ++i)
+    {
+      // ROS_DEBUG_STREAM_NAMED(name_,"Creating ik solver " << i);
+      kin_solvers_[arm_jmg->getName()].push_back(arm_jmg->getSolverInstance());
+
+      // Test to make sure we have a valid kinematics solver
+      if (!kin_solvers_[arm_jmg->getName()][i])
+      {
+        ROS_ERROR_STREAM_NAMED(name_, "No kinematic solver found");
+        return 0;
+      }
+    }
+  }
+
+  // Robot states
+  // Create a robot state for every thread
+  if (robot_states_.size() != num_threads)
+  {
+    robot_states_.clear();
+    for (std::size_t i = 0; i < num_threads; ++i)
+    {
+      // Copy the previous robot state
+      robot_states_.push_back(std::make_shared<robot_state::RobotState>(*robot_state_));
+    }
+  }
+  else  // update the states
+  {
+    for (std::size_t i = 0; i < num_threads; ++i)
+    {
+      // Copy the previous robot state
+      *(robot_states_[i]) = *robot_state_;
+    }
+  }
+
+  // Transform poses
+  // bring the pose to the frame of the IK solver
+  const std::string& ik_frame = kin_solvers_[arm_jmg->getName()][0]->getBaseFrame();
+  Eigen::Isometry3d link_transform;
+  ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug",
+                         "Frame transform from ik_frame: " << ik_frame << " and robot model frame: "
+                                                           << robot_state_->getRobotModel()->getModelFrame());
+  if (!moveit::core::Transforms::sameFrame(ik_frame, robot_state_->getRobotModel()->getModelFrame()))
+  {
+    const robot_model::LinkModel* lm =
+        robot_state_->getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame);
+
+    if (!lm)
+    {
+      ROS_ERROR_STREAM_NAMED(name_, "Unable to find frame for link transform");
+      return 0;
+    }
+
+    link_transform = robot_state_->getGlobalLinkTransform(lm).inverse();
+  }
+
+  // Ensure the ACM entries are set to ignore collisions between the eef and the target object
+  if (!target_object_id.empty())
+  {
+    if (!planning_scene_clone->knowsFrameTransform(target_object_id))
+    {
+      ROS_ERROR_STREAM_NAMED(name_, "target_object_id: " << target_object_id << " unknown to the planning scene");
+      return 0;
+    }
+    std::vector<std::string> ee_links = grasp_candidates.front()->grasp_data_->ee_jmg_->getLinkModelNames();
+    if (!ee_links.empty())
+    {
+      setACMFingerEntry(target_object_id, true, ee_links, planning_scene_clone);
+    }
+  }
+
+  // Create the seed state vector
+  std::vector<double> ik_seed_state;
+  seed_state->copyJointGroupPositions(arm_jmg, ik_seed_state);
+
+  // Thread data
+  // Allocate only once to increase performance
+  std::vector<IkThreadStructPtr> ik_thread_structs;
+  ik_thread_structs.resize(num_threads);
+  for (std::size_t thread_id = 0; thread_id < num_threads; ++thread_id)
+  {
+    ik_thread_structs[thread_id] =
+        std::make_shared<IkThreadStruct>(grasp_candidates, planning_scene_clone, link_transform,
+                                         0,  // this is filled in by OpenMP
+                                         kin_solvers_[arm_jmg->getName()][thread_id], robot_states_[thread_id],
+                                         solver_timeout_, filter_pregrasp, visualize, thread_id, target_object_id);
+    ik_thread_structs[thread_id]->ik_seed_state_ = ik_seed_state;
+  }
+
+  // Benchmark time
+  ros::Time start_time;
+  start_time = ros::Time::now();
+
+  // Loop through poses and find those that are kinematically feasible
+
+  omp_set_num_threads(num_threads);
+#pragma omp parallel for schedule(dynamic)
+  for (std::size_t grasp_id = 0; grasp_id < grasp_candidates.size(); ++grasp_id)
+  {
+    std::size_t thread_id = omp_get_thread_num();
+    ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Thread " << thread_id << " processing grasp " << grasp_id);
+
+    // If in verbose mode allow for quick exit
+    if (ik_thread_structs[thread_id]->visual_debug_ && !ros::ok())
+      continue;  // breaking a for loop is not allows with OpenMP
+
+    // Assign grasp to process
+    ik_thread_structs[thread_id]->grasp_id = grasp_id;
+
+    // Process the grasp if it hasn't already been filtered out
+    if (grasp_candidates[grasp_id]->isValid())
+      processCandidateGrasp(ik_thread_structs[thread_id]);
+  }
+
+  if (statistics_verbose_)
+  {
+    // End Benchmark time
+    double duration = (ros::Time::now() - start_time).toSec();
+    ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "===================================================");
+    ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "FILTER DURATION");
+    ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "Grasp Filter Duration :\t" << duration);
+    ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "---------------------------------------------------");
+  }
+
+  std::size_t not_filtered = 0;
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+    if (grasp_candidates[i]->isValid())
+      ++not_filtered;
+
+  return not_filtered;
+}
+
+void GraspFilter::printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const
+{
+  if (!statistics_verbose_)
+    return;
+  static const std::string logger_name = name_ + ".filter_statistics";
+  // Count number of grasps remaining
+  std::size_t not_filtered = 0;
+  std::size_t grasp_filtered_by_ik = 0;
+  std::size_t grasp_filtered_by_cutting_plane = 0;
+  std::size_t grasp_filtered_by_orientation = 0;
+  std::size_t pregrasp_filtered_by_ik = 0;
+
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+  {
+    if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK)
+      ++grasp_filtered_by_ik;
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE)
+      ++grasp_filtered_by_cutting_plane;
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION)
+      ++grasp_filtered_by_orientation;
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK)
+      ++pregrasp_filtered_by_ik;
+    else if (grasp_candidates[i]->isValid())
+      ++not_filtered;
+  }
+
+  ROS_INFO_STREAM_NAMED(logger_name, "=======================================================");
+  ROS_INFO_STREAM_NAMED(logger_name, "GRASP FILTER RESULTS ");
+  ROS_INFO_STREAM_NAMED(logger_name, "Total candidate grasps          " << grasp_candidates.size());
+  ROS_INFO_STREAM_NAMED(logger_name, "Total valid grasps              " << not_filtered);
+  ROS_INFO_STREAM_NAMED(logger_name, "-------------------------------------------------------");
+  ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_cutting_plane " << grasp_filtered_by_cutting_plane);
+  ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_orientation   " << grasp_filtered_by_orientation);
+  ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_ik            " << grasp_filtered_by_ik);
+  ROS_INFO_STREAM_NAMED(logger_name, "pregrasp_filtered_by_ik         " << pregrasp_filtered_by_ik);
+}
+
+bool GraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct)
+{
+  ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Checking grasp #" << ik_thread_struct->grasp_id);
+
+  // Helper pointer
+  GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id];
+
+  // Get pose
+  ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose;
+
+  // Filter by cutting planes
+  for (auto& cutting_plane : cutting_planes_)
+  {
+    if (filterGraspByPlane(grasp_candidate, cutting_plane->pose_, cutting_plane->plane_, cutting_plane->direction_))
+    {
+      return false;
+    }
+  }
+
+  // Filter by desired orientation
+  for (auto& desired_grasp_orientation : desired_grasp_orientations_)
+  {
+    if (filterGraspByOrientation(grasp_candidate, desired_grasp_orientation->pose_,
+                                 desired_grasp_orientation->max_angle_offset_))
+    {
+      return false;
+    }
+  }
+
+  std::vector<double> grasp_ik_solution;
+  if (filterGraspByGraspIK(grasp_candidate, grasp_ik_solution, ik_thread_struct))
+  {
+    return false;
+  }
+
+  // Assign the grasp_ik solution.
+  // TODO: This should be moved out of filtering
+  grasp_candidate->grasp_ik_solution_ = grasp_ik_solution;
+
+  // Copy solution to seed state so that next solution is faster
+  ik_thread_struct->ik_seed_state_ = grasp_ik_solution;
+
+  std::vector<double> pregrasp_ik_solution;
+  if (filterGraspByPreGraspIK(grasp_candidate, pregrasp_ik_solution, ik_thread_struct))
+  {
+    return false;
+  }
+  // Assign the pregrasp_ik solution.
+  // TODO: This should be moved out of filtering
+  grasp_candidate->pregrasp_ik_solution_ = pregrasp_ik_solution;
+
+  return true;
+}
+
+bool GraspFilter::findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
+                                 const GraspCandidatePtr& grasp_candidate,
+                                 const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const
+{
+  robot_state::RobotState state = ik_thread_struct->planning_scene_->getCurrentState();
+  if (ik_thread_struct->ik_seed_state_.size() == grasp_candidate->grasp_data_->arm_jmg_->getActiveJointModels().size())
+  {
+    state.setJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_seed_state_);
+    state.update();
+  }
+
+  // Copy in attached bodies which get removed by setJointGroupPositions for some reason
+  std::vector<const robot_state::AttachedBody*> attached_bodies;
+  ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies);
+
+  for (const robot_state::AttachedBody* ab : attached_bodies)
+    state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(),
+                     ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframes());
+
+  bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose,
+                                    ik_thread_struct->timeout_, constraint_fn);
+
+  // Results
+  if (ik_success)
+  {
+    state.copyJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_solution);
+    return true;
+  }
+  else
+  {
+    // The grasp was valid but the pre-grasp was not
+    ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "IK solution not found");
+    return false;
+  }
+}
+
+void GraspFilter::addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction)
+{
+  cutting_planes_.push_back(std::make_shared<CuttingPlane>(pose, plane, direction));
+}
+
+void GraspFilter::addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset)
+{
+  desired_grasp_orientations_.push_back(std::make_shared<DesiredGraspOrientation>(pose, max_angle_offset));
+}
+
+bool GraspFilter::removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const
+{
+  std::size_t original_num_grasps = grasp_candidates.size();
+
+  // Remove all invalid grasps
+  for (std::vector<GraspCandidatePtr>::iterator it = grasp_candidates.begin(); it != grasp_candidates.end();)
+  {
+    // Check if valid grasp
+    if (!(*it)->isValid())
+    {
+      it = grasp_candidates.erase(it);  // not valid
+    }
+    else
+    {
+      ++it;  // was valid, keep
+    }
+  }
+  ROS_INFO_STREAM_NAMED(name_, "Removed " << original_num_grasps - grasp_candidates.size()
+                                          << " invalid grasp candidates, " << grasp_candidates.size() << " remaining");
+
+  // Error Check
+  if (grasp_candidates.empty())
+  {
+    ROS_WARN_STREAM_NAMED(name_, "No remaining grasp candidates");
+    return false;
+  }
+
+  // Order remaining valid grasps by best score
+  std::sort(grasp_candidates.begin(), grasp_candidates.end(), compareGraspScores);
+
+  ROS_INFO_STREAM_NAMED(name_, "Sorted valid grasps, highest quality is "
+                                   << grasp_candidates.front()->grasp_.grasp_quality << " and lowest quality is "
+                                   << grasp_candidates.back()->grasp_.grasp_quality);
+
+  return true;
+}
+
+bool GraspFilter::visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                                  const moveit::core::JointModelGroup* arm_jmg)
+{
+  // Publish in batch
+  visual_tools_->enableBatchPublishing(true);
+
+  /*
+    NOTE: duplicated in README.md
+    MAGENTA - grasp filtered by cutting plane
+    YELLOW - grasp filtered by orientation
+    RED - grasp filtered by ik
+    PINK - grasp filtered by collision
+    BLUE - pregrasp filtered by ik
+    CYAN - pregrasp filtered by collision
+    GREY - misc filtered
+    GREEN - valid
+  */
+  ROS_INFO_STREAM_NAMED(name_, "Showing " << grasp_candidates.size() << " solutions at a speed of "
+                                          << show_filtered_arm_solutions_speed_ << "sec per solution");
+  ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------");
+  ROS_INFO_STREAM_NAMED(name_, "   MAGENTA - grasp filtered by cutting plane");
+  ROS_INFO_STREAM_NAMED(name_, "   YELLOW  - grasp filtered by orientation");
+  ROS_INFO_STREAM_NAMED(name_, "   RED     - grasp filtered by ik");
+  ROS_INFO_STREAM_NAMED(name_, "   PINK    - grasp filtered by collision");
+  ROS_INFO_STREAM_NAMED(name_, "   BLUE    - pregrasp filtered by ik");
+  ROS_INFO_STREAM_NAMED(name_, "   CYAN    - pregrasp filtered by collision");
+  ROS_INFO_STREAM_NAMED(name_, "   GREY    - misc filtered");
+  ROS_INFO_STREAM_NAMED(name_, "   GREEN   - valid");
+  ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------");
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+  {
+    double size = 0.03;  // 0.01 * grasp_candidates[i]->grasp_.grasp_quality;
+
+    if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK)
+    {
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::RED,
+                                   rviz_visual_tools::SMALL, size);
+    }
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK)
+    {
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::BLUE,
+                                   rviz_visual_tools::SMALL, size);
+    }
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE)
+    {
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::MAGENTA,
+                                   rviz_visual_tools::SMALL, size);
+    }
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION)
+    {
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::YELLOW,
+                                   rviz_visual_tools::SMALL, size);
+    }
+    else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED)
+    {
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREEN,
+                                   rviz_visual_tools::SMALL, size);
+    }
+    else
+      visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREY,
+                                   rviz_visual_tools::SMALL, size);
+
+    // Publish in batch
+    visual_tools_->trigger();
+    ros::Duration(.01).sleep();
+  }
+
+  return true;
+}
+
+bool GraspFilter::visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                                       const moveit::core::JointModelGroup* arm_jmg, double animation_speed)
+{
+  // Convert the grasp_candidates into a format moveit_visual_tools can use
+  std::vector<trajectory_msgs::JointTrajectoryPoint> ik_solutions;
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+  {
+    // Check if valid grasp
+    if (!grasp_candidates[i]->isValid())
+      continue;
+
+    trajectory_msgs::JointTrajectoryPoint new_point;
+    new_point.positions = grasp_candidates[i]->grasp_ik_solution_;
+    ik_solutions.push_back(new_point);
+  }
+
+  if (ik_solutions.empty())
+  {
+    ROS_ERROR_STREAM_NAMED(name_, "Unable to visualize IK solutions because there are no valid ones");
+    return false;
+  }
+
+  return visual_tools_->publishIKSolutions(ik_solutions, arm_jmg, animation_speed);
+}
+
+bool GraspFilter::visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates)
+{
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+  {
+    // Check if valid grasp
+    if (!grasp_candidates[i]->isValid())
+      continue;
+
+    // Apply the pregrasp state
+    if (grasp_candidates[i]->getPreGraspState(robot_state_))
+    {
+      // Show in Rviz
+      visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::ORANGE);
+      visual_tools_->trigger();
+      ros::Duration(show_filtered_arm_solutions_pregrasp_speed_).sleep();
+    }
+
+    // Apply the grasp state
+    if (grasp_candidates[i]->getGraspStateClosed(robot_state_))
+    {
+      // Show in Rviz
+      visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::WHITE);
+      visual_tools_->trigger();
+      ros::Duration(show_filtered_arm_solutions_speed_).sleep();
+    }
+  }
+
+  return true;
+}
+
+bool GraspFilter::visualizeCuttingPlanes()
+{
+  if (show_cutting_planes_)
+  {
+    for (auto& cutting_plane : cutting_planes_)
+    {
+      switch (cutting_plane->plane_)
+      {
+        case XY:
+          visual_tools_->publishXYPlane(cutting_plane->pose_);
+          break;
+        case XZ:
+          visual_tools_->publishXZPlane(cutting_plane->pose_);
+          break;
+        case YZ:
+          visual_tools_->publishYZPlane(cutting_plane->pose_);
+          break;
+        default:
+          ROS_ERROR_STREAM_NAMED(name_, "Unknown cutting plane type");
+      }
+    }
+    visual_tools_->trigger();
+  }
+  return true;
+}
+
+void GraspFilter::clearCuttingPlanes()
+{
+  cutting_planes_.clear();
+}
+
+void GraspFilter::clearDesiredGraspOrientations()
+{
+  desired_grasp_orientations_.clear();
+}
+
+bool GraspFilter::addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product,
+                                         const double& bin_width, const double& bin_height)
+{
+  // Add grasp filters
+  clearCuttingPlanes();
+  clearDesiredGraspOrientations();
+
+  // Bottom of bin
+  addCuttingPlane(world_to_bin, XY, -1);
+
+  // Right wall of bin
+  addCuttingPlane(world_to_bin, XZ, -1);
+
+  // Top of bin
+  Eigen::Isometry3d world_to_bin_top = world_to_bin;
+  world_to_bin_top.translation() += Eigen::Vector3d(0, bin_width, bin_height);
+  addCuttingPlane(world_to_bin_top, XY, 1);
+
+  // Left wall of bin
+  addCuttingPlane(world_to_bin_top, XZ, 1);
+
+  // // Back half of product
+  Eigen::Isometry3d world_to_bin_back = world_to_bin;
+  world_to_bin_back.translation() +=
+      Eigen::Vector3d(bin_to_product.translation().x(), bin_width / 2.0, bin_height / 2.0);
+  addCuttingPlane(world_to_bin_back, YZ, 1);
+
+  return true;
+}
+
+void GraspFilter::setACMFingerEntry(const std::string& object_name, bool allowed,
+                                    const std::vector<std::string>& ee_link_names,
+                                    const planning_scene::PlanningScenePtr& scene)
+{
+  static const std::string logger_name = name_ + ".set_acm_finger_entry";
+  ROS_DEBUG_STREAM_NAMED(logger_name, "" << object_name.c_str() << ", " << (allowed ? "true" : "false"));
+
+  // Lock planning scene
+  for (std::size_t i = 0; i < ee_link_names.size(); ++i)
+  {
+    ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : %s", object_name.c_str(), ee_link_names[i].c_str(),
+                    allowed ? "allowed" : "not allowed");
+    scene->getAllowedCollisionMatrixNonConst().setEntry(object_name, ee_link_names[i], allowed);
+  }
+
+  // Debug current matrix
+  if (false)
+  {
+    moveit_msgs::AllowedCollisionMatrix msg;
+    scene->getAllowedCollisionMatrix().getMessage(msg);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "Current collision matrix: " << msg);
+  }
+}
+
+void GraspFilter::publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const
+{
+  moveit_msgs::PlanningScene msg;
+  ps->getPlanningSceneMsg(msg);
+  planning_scene_publisher_.publish(msg);
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/grasp_generator.cpp b/src/mtc/src/grasp_generator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..95b469ba40a6db936dd90b72708e79fe7b13dabc
--- /dev/null
+++ b/src/mtc/src/grasp_generator.cpp
@@ -0,0 +1,199 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
+   Desc:   Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
+*/
+
+#include "../include/grasp_generator.h"
+#include "../include/grasp_filter.h"
+
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+namespace
+{
+void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width,
+                            double grasp_padding_on_approach)
+{
+  ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to "
+                                                << percent_open << " % open. Stats:"
+                                                << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach
+                                                << "\n object_width: \t " << object_width
+                                                << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach);
+}
+
+}  // namespace
+
+namespace moveit_grasps
+
+{
+// Constructor
+GraspGenerator::GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose)
+  : ideal_grasp_pose_(Eigen::Isometry3d::Identity())
+  , visual_tools_(visual_tools)
+  , verbose_(verbose)
+  , nh_("~/moveit_grasps/generator")
+  , grasp_score_weights_(std::make_shared<GraspScoreWeights>())
+{
+  // Load visulization settings
+  const std::string parent_name = "grasps";  // for namespacing logging messages
+  std::size_t error = 0;
+
+  error += !rosparam_shortcuts::get(parent_name, nh_, "verbose", verbose_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps", show_prefiltered_grasps_);
+  error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps_speed", show_prefiltered_grasps_speed_);
+
+  // Load scoring weights
+  rosparam_shortcuts::shutdownIfError(parent_name, error);
+}
+
+void GraspGenerator::setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy)
+{
+  ROS_ASSERT_MSG(ideal_grasp_orientation_rpy.size() == 3, "setIdealTCPGraspPoseRPY must be set with a vector of length "
+                                                          "3");
+
+  // copy the ideal_grasp_pose.translation() so that we only change the orientation.
+  Eigen::Vector3d ideal_grasp_pose_translation(ideal_grasp_pose_.translation());
+
+  // Set ideal grasp pose (currently only uses orientation of pose)
+  ideal_grasp_pose_ = Eigen::Isometry3d::Identity() *
+                      Eigen::AngleAxisd(ideal_grasp_orientation_rpy[0], Eigen::Vector3d::UnitX()) *
+                      Eigen::AngleAxisd(ideal_grasp_orientation_rpy[1], Eigen::Vector3d::UnitY()) *
+                      Eigen::AngleAxisd(ideal_grasp_orientation_rpy[2], Eigen::Vector3d::UnitZ());
+
+  ideal_grasp_pose_.translation() = ideal_grasp_pose_translation;
+}
+
+Eigen::Vector3d GraspGenerator::getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link)
+{
+  // Grasp Pose Variables
+  Eigen::Isometry3d grasp_pose_eef_mount_eigen;
+  tf::poseMsgToEigen(grasp.grasp_pose.pose, grasp_pose_eef_mount_eigen);
+
+  // The direction of the pre-grasp in the frame of the parent link
+  Eigen::Vector3d pre_grasp_approach_direction =
+      Eigen::Vector3d(grasp.pre_grasp_approach.direction.vector.x, grasp.pre_grasp_approach.direction.vector.y,
+                      grasp.pre_grasp_approach.direction.vector.z);
+
+  // Decide if we need to change the approach_direction to the local frame of the end effector orientation
+  if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link)
+  {
+    // Apply/compute the approach_direction vector in the local frame of the grasp_pose orientation
+    return grasp_pose_eef_mount_eigen.rotation() * pre_grasp_approach_direction;
+  }
+  return pre_grasp_approach_direction;
+}
+
+geometry_msgs::PoseStamped GraspGenerator::getPreGraspPose(const GraspCandidatePtr& grasp_candidate,
+                                                           const std::string& ee_parent_link)
+{
+  // Grasp Pose Variables
+  Eigen::Isometry3d grasp_pose_eef_mount_eigen;
+  tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose_eef_mount_eigen);
+
+  // Get pre-grasp pose first
+  Eigen::Isometry3d pre_grasp_pose_eef_mount_eigen =
+      grasp_pose_eef_mount_eigen;  // Copy original grasp pose to pre-grasp pose
+
+  // Approach direction
+  Eigen::Vector3d pre_grasp_approach_direction_local = getPreGraspDirection(grasp_candidate->grasp_, ee_parent_link);
+
+  // Update the grasp matrix usign the new locally-framed approach_direction
+  pre_grasp_pose_eef_mount_eigen.translation() -=
+      pre_grasp_approach_direction_local * grasp_candidate->grasp_.pre_grasp_approach.desired_distance;
+
+  // Convert eigen pre-grasp position back to regular message
+  geometry_msgs::PoseStamped pre_grasp_pose_eef_mount_msg;
+  tf::poseEigenToMsg(pre_grasp_pose_eef_mount_eigen, pre_grasp_pose_eef_mount_msg.pose);
+
+  // Copy original header to new grasp
+  pre_grasp_pose_eef_mount_msg.header = grasp_candidate->grasp_.grasp_pose.header;
+
+  return pre_grasp_pose_eef_mount_msg;
+}
+
+void GraspGenerator::getGraspWaypoints(const GraspCandidatePtr& grasp_candidate,
+                                       EigenSTL::vector_Isometry3d& grasp_waypoints)
+{
+  Eigen::Isometry3d grasp_pose;
+  tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose);
+
+  const geometry_msgs::PoseStamped pregrasp_pose_msg =
+      GraspGenerator::getPreGraspPose(grasp_candidate, grasp_candidate->grasp_data_->parent_link_->getName());
+
+  // Create waypoints
+  Eigen::Isometry3d pregrasp_pose;
+  tf::poseMsgToEigen(pregrasp_pose_msg.pose, pregrasp_pose);
+
+  Eigen::Isometry3d lifted_grasp_pose = grasp_pose;
+  lifted_grasp_pose.translation().z() += grasp_candidate->grasp_data_->lift_distance_desired_;
+
+  // Solve for post grasp retreat
+  Eigen::Isometry3d retreat_pose = lifted_grasp_pose;
+  Eigen::Vector3d postgrasp_vector(grasp_candidate->grasp_.post_grasp_retreat.direction.vector.x,
+                                   grasp_candidate->grasp_.post_grasp_retreat.direction.vector.y,
+                                   grasp_candidate->grasp_.post_grasp_retreat.direction.vector.z);
+  postgrasp_vector.normalize();
+
+  retreat_pose.translation() +=
+      retreat_pose.rotation() * postgrasp_vector * grasp_candidate->grasp_.post_grasp_retreat.desired_distance;
+
+  grasp_waypoints.clear();
+  grasp_waypoints.resize(4);
+  grasp_waypoints[0] = pregrasp_pose;
+  grasp_waypoints[1] = grasp_pose;
+  grasp_waypoints[2] = lifted_grasp_pose;
+  grasp_waypoints[3] = retreat_pose;
+}
+
+void GraspGenerator::publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data,
+                                       const rviz_visual_tools::colors& color, double approach_length)
+{
+  visual_tools_->publishArrow(grasp, color, rviz_visual_tools::MEDIUM);
+}
+
+bool GraspGenerator::visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
+                                             const moveit::core::JointModelGroup* ee_jmg, double animation_speed)
+{
+  // Convert the grasp_candidates into a format moveit_visual_tools can use
+  std::vector<moveit_msgs::Grasp> grasps;
+  for (std::size_t i = 0; i < grasp_candidates.size(); ++i)
+  {
+    grasps.push_back(grasp_candidates[i]->grasp_);
+  }
+
+  return visual_tools_->publishAnimatedGrasps(grasps, ee_jmg, show_prefiltered_grasps_speed_);
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/grasp_pipeline.cpp b/src/mtc/src/grasp_pipeline.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9298095a78636464955600c86d8e162b2483e48
--- /dev/null
+++ b/src/mtc/src/grasp_pipeline.cpp
@@ -0,0 +1,695 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, PickNik LLC
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ ******************************/
+
+// ROS
+#include <ros/ros.h>
+
+// MoveIt
+#include <moveit/robot_state/robot_state.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_pipeline/planning_pipeline.h>
+#include <moveit/robot_state/conversions.h>
+
+// Visualization
+#include <moveit_visual_tools/moveit_visual_tools.h>
+
+// Grasp
+#include "../include/two_finger_grasp_generator.h"
+#include "../include/two_finger_grasp_data.h"
+#include "../include/two_finger_grasp_filter.h"
+#include "../include/grasp_planner.h"
+
+
+#include <fstream>
+#include "yaml-cpp/yaml.h"
+#include "tf2_eigen/tf2_eigen.h"
+
+
+// Parameter loading
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+
+YAML::Node node;
+
+namespace moveit_grasps
+{
+static const std::string LOGNAME = "grasp_pipeline_demo";
+
+namespace
+{
+bool isStateValid(const planning_scene::PlanningScene* planning_scene,
+                  const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state,
+                  const robot_model::JointModelGroup* group, const double* ik_solution)
+{
+  robot_state->setJointGroupPositions(group, ik_solution);
+  robot_state->update();
+  return !planning_scene->isStateColliding(*robot_state, group->getName());
+}
+
+void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
+{
+  visual_tools->prompt(prompt);
+}
+
+}  // namespace
+
+class GraspPipeline
+{
+public:
+  // Constructor
+  GraspPipeline() : nh_("~")
+  {
+    // Get arm info from param server
+    const std::string parent_name = "grasp_filter_demo";  // for namespacing logging messages
+    rosparam_shortcuts::get(parent_name, nh_, "planning_group_name", planning_group_name_);
+    rosparam_shortcuts::get(parent_name, nh_, "ee_group_name", ee_group_name_);
+
+    ROS_INFO_STREAM_NAMED("test", "End Effector: " << ee_group_name_);
+    ROS_INFO_STREAM_NAMED("test", "Planning Group: " << planning_group_name_);
+
+    loadScene();
+    setupGraspPipeline();
+  }
+
+  void loadScene()
+  {
+    // ---------------------------------------------------------------------------------------------
+    // Load planning scene to share
+    planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+    if (!planning_scene_monitor_->getPlanningScene())
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured");
+      return;
+    }
+    planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
+                                                          "grasping_planning_scene");
+    planning_scene_monitor_->getPlanningScene()->setName("grasping_planning_scene");
+
+    robot_model_loader::RobotModelLoaderPtr robot_model_loader;
+    robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description");
+
+    // Load the robot model
+    robot_model_ = robot_model_loader->getModel();
+    arm_jmg_ = robot_model_->getJointModelGroup(planning_group_name_);
+
+    // ---------------------------------------------------------------------------------------------
+    // Load the Robot Viz Tools for publishing to Rviz
+    visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
+        robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_);
+    visual_tools_->loadMarkerPub();
+    visual_tools_->loadRobotStatePub("/display_robot_state");
+    visual_tools_->loadTrajectoryPub("/display_planned_path");
+    visual_tools_->loadSharedRobotState();
+    visual_tools_->enableBatchPublishing();
+    visual_tools_->deleteAllMarkers();
+    visual_tools_->removeAllCollisionObjects();
+    visual_tools_->hideRobot();
+    visual_tools_->trigger();
+
+    // Publish the global frame
+    visual_tools_->publishAxis(Eigen::Isometry3d::Identity());
+    visual_tools_->trigger();
+  }
+
+  void setupGraspPipeline()
+  {
+    // ---------------------------------------------------------------------------------------------
+    // Load grasp data specific to our robot
+    grasp_data_ =
+        std::make_shared<moveit_grasps::TwoFingerGraspData>(nh_, ee_group_name_, visual_tools_->getRobotModel());
+    if (!grasp_data_->loadGraspData(nh_, ee_group_name_))
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to load Grasp Data parameters.");
+      exit(-1);
+    }
+
+    // ---------------------------------------------------------------------------------------------
+    // Load grasp generator
+    grasp_generator_ = std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_);
+
+    // Set the ideal grasp orientation for scoring
+    std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 };
+    grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy);
+
+    // Set custom grasp score weights
+    auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>();
+    grasp_score_weights->orientation_x_score_weight_ = 2.0;
+    grasp_score_weights->orientation_y_score_weight_ = 2.0;
+    grasp_score_weights->orientation_z_score_weight_ = 2.0;
+    grasp_score_weights->translation_x_score_weight_ = 1.0;
+    grasp_score_weights->translation_y_score_weight_ = 1.0;
+    grasp_score_weights->translation_z_score_weight_ = 1.0;
+    // Finger gripper specific weights.
+    grasp_score_weights->depth_score_weight_ = 2.0;
+    grasp_score_weights->width_score_weight_ = 2.0;
+    // Assign the grasp score weights in the grasp_generator
+    grasp_generator_->setGraspScoreWeights(grasp_score_weights);
+
+    // ---------------------------------------------------------------------------------------------
+    // Load grasp filter
+    grasp_filter_ =
+        std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_);
+
+    // ---------------------------------------------------------------------------------------------
+    // Load grasp planner for approach, lift and retreat planning
+    grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);
+
+    // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
+    grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1));
+
+    // -----------------------------------------------------
+    // Load the motion planning pipeline
+    planning_pipeline_ =
+        std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, nh_, "planning_plugin", "request_adapter");
+  }
+
+  bool demoRandomGrasp()
+  {
+    // -----------------------------------
+    // Generate random object to grasp
+    geometry_msgs::Pose object_pose;
+    double object_x_depth;
+    double object_y_width;
+    double object_z_height;
+    std::string object_name;
+    if (!generateRandomCuboid(object_name, object_pose, object_x_depth, object_y_width, object_z_height))
+    {
+      ROS_ERROR_NAMED(LOGNAME, "Failed to add random cuboid ot planning scene");
+      return false;
+    }
+
+    // -----------------------------------
+    // Generate grasp candidates
+    std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
+
+    // Configure the desired types of grasps
+    moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
+        moveit_grasps::TwoFingerGraspCandidateConfig();
+    grasp_generator_config.disableAll();
+    grasp_generator_config.enable_face_grasps_ = true;
+    grasp_generator_config.generate_y_axis_grasps_ = true;
+    grasp_generator_config.generate_x_axis_grasps_ = true;
+    grasp_generator_config.generate_z_axis_grasps_ = true;
+
+    grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
+    if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), object_x_depth, object_y_width,
+                                          object_z_height, grasp_data_, grasp_candidates))
+    {
+      ROS_ERROR_NAMED(LOGNAME, "Grasp generator failed to generate any valid grasps");
+      return false;
+    }
+
+    // --------------------------------------------
+    // Generating a seed state for filtering grasps
+    robot_state::RobotStatePtr seed_state =
+        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
+    Eigen::Isometry3d eef_mount_grasp_pose =
+        visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
+    if (!getIKSolution(arm_jmg_, eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
+    {
+      ROS_WARN_STREAM_NAMED(LOGNAME, "The ideal seed state is not reachable. Using start state as seed.");
+    }
+
+    // --------------------------------------------
+    // Filtering grasps
+    // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
+    bool filter_pregrasps = true;
+    if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, arm_jmg_, seed_state, filter_pregrasps,
+                                     object_name))
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Filter grasps failed");
+      return false;
+    }
+    if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates))
+    {
+      ROS_WARN_NAMED(LOGNAME, "Grasp filtering removed all grasps");
+      return false;
+    }
+    ROS_INFO_STREAM_NAMED(LOGNAME, "" << grasp_candidates.size() << " remain after filtering");
+
+    // Plan free-space approach, cartesian approach, lift and retreat trajectories
+    moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
+    moveit_msgs::MotionPlanResponse pre_approach_plan;
+    if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name))
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions");
+      return false;
+    }
+
+    visualizePick(selected_grasp_candidate, pre_approach_plan);
+
+    return true;
+  }
+
+  void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
+                     const moveit_msgs::MotionPlanResponse& pre_approach_plan)
+  {
+    EigenSTL::vector_Isometry3d waypoints;
+    moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints);
+
+    // Visualize waypoints
+    visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
+    visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
+    visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
+    visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
+    visual_tools_->trigger();
+
+    // Get the pre and post grasp states
+    visual_tools_->prompt("pre_grasp");
+    robot_state::RobotStatePtr pre_grasp_state =
+        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
+    valid_grasp_candidate->getPreGraspState(pre_grasp_state);
+    visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE);
+    robot_state::RobotStatePtr grasp_state =
+        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
+    if (valid_grasp_candidate->getGraspStateClosed(grasp_state))
+    {
+      visual_tools_->prompt("grasp");
+      visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW);
+    }
+    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 &&
+        valid_grasp_candidate->segmented_cartesian_traj_[1].size())
+    {
+      visual_tools_->prompt("lift");
+      visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(),
+                                       rviz_visual_tools::BLUE);
+    }
+    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 &&
+        valid_grasp_candidate->segmented_cartesian_traj_[2].size())
+    {
+      visual_tools_->prompt("retreat");
+      visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(),
+                                       rviz_visual_tools::PURPLE);
+    }
+
+    visual_tools_->prompt("show free space approach");
+    visual_tools_->hideRobot();
+    visual_tools_->trigger();
+
+    bool wait_for_animation = true;
+    visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation);
+    ros::Duration(0.25).sleep();
+    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH)
+      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH],
+                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+    ros::Duration(0.25).sleep();
+
+    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT)
+      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT],
+                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+    ros::Duration(0.25).sleep();
+
+    if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT)
+      visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT],
+                                           valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+    ros::Duration(0.25).sleep();
+  }
+
+  bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates,
+                     moveit_grasps::GraspCandidatePtr& valid_grasp_candidate,
+                     moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name)
+  {
+    moveit::core::RobotStatePtr current_state;
+    {
+      boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
+          new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
+      current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState());
+    }
+
+    bool success = false;
+    for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin()))
+    {
+      valid_grasp_candidate = grasp_candidates.front();
+      valid_grasp_candidate->getPreGraspState(current_state);
+      if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false,
+                                                   object_name))
+      {
+        ROS_INFO_NAMED(LOGNAME, "failed to plan approach lift retreat");
+        continue;
+      }
+
+      robot_state::RobotStatePtr pre_grasp_state =
+          valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front();
+      if (!planPreApproach(*pre_grasp_state, pre_approach_plan))
+      {
+        ROS_WARN_NAMED(LOGNAME, "failed to plan to pregrasp_state");
+        continue;
+      }
+
+      success = true;
+      break;
+    }
+    return success;
+  }
+
+  bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan)
+  {
+    planning_interface::MotionPlanRequest req;
+    planning_interface::MotionPlanResponse res;
+
+    double tolerance_above = 0.01;
+    double tolerance_below = 0.01;
+    // req.planner_id = "RRTConnectkConfigDefault";
+    req.group_name = arm_jmg_->getName();
+    req.num_planning_attempts = 5;
+    req.allowed_planning_time = 1.5;
+    moveit_msgs::Constraints goal =
+        kinematic_constraints::constructGoalConstraints(goal_state, arm_jmg_, tolerance_below, tolerance_above);
+
+    req.goal_constraints.push_back(goal);
+    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
+        new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
+
+    // ---------------------------------
+    // Change the robot current state
+    // NOTE: We have to do this since Panda start configuration is in self collision.
+    robot_state::RobotState rs = (*ls)->getCurrentState();
+    std::vector<double> starting_joint_values = { 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
+    std::vector<std::string> joint_names = { "panda_joint1", "panda_joint2", "panda_joint3", "panda_joint4",
+                                             "panda_joint5", "panda_joint6", "panda_joint7" };
+    // arm_jmg_->getActiveJointModelNames();
+    for (std::size_t i = 0; i < joint_names.size(); ++i)
+    {
+      rs.setJointPositions(joint_names[i], &starting_joint_values[i]);
+    }
+    rs.update();
+    robot_state::robotStateToRobotStateMsg(rs, req.start_state);
+    // ---------------------------
+
+    planning_pipeline_->generatePlan(*ls, req, res);
+    if (res.error_code_.val != res.error_code_.SUCCESS)
+    {
+      ROS_INFO_NAMED(LOGNAME, "Failed to plan approach successfully");
+      return false;
+    }
+
+    res.getMessage(pre_approach_plan);
+    return true;
+  }
+
+  bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose,
+                     robot_state::RobotState& solution, const std::string& link_name)
+  {
+    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls(
+        new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_));
+
+    moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind(
+        &isStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3);
+
+    // seed IK call with current state
+    solution = (*ls)->getCurrentState();
+
+
+    // Solve IK problem for arm
+    // disable explicit restarts to guarantee close solution if one exists
+    const double timeout = 0.1;
+
+
+
+
+    
+    if (solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn)) {
+      geometry_msgs::Pose target = tf2::toMsg(target_pose);
+      node["data"].push_back(std::string(
+            std::to_string(target.position.x) + " " 
+          + std::to_string(target.position.y) + " " 
+          + std::to_string(target.position.z) + " "
+          + std::to_string(target.orientation.x) + " " 
+          + std::to_string(target.orientation.y) + " " 
+          + std::to_string(target.orientation.z) + " " 
+          + std::to_string(target.orientation.w) 
+          ));
+      return true;
+    } 
+    return false;
+  }
+
+  bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth,
+                            double& y_width, double& z_height)
+  {
+
+    // Generate random cuboid
+    double xmin = 0.5;
+    double xmax = 0.7;
+    double ymin = -0.25;
+    double ymax = 0.25;
+    double zmin = 0.2;
+    double zmax = 0.7;
+    rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
+
+    double cuboid_size_min = 0.01;
+    double cuboid_size_max = 0.03;
+    rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);
+
+    object_name = "pick_target";
+    visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds);
+    visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED);
+    visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
+    visual_tools_->trigger();
+
+    bool success = true;
+    double timeout = 5;  // seconds
+    ros::Rate rate(100);
+    while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name))
+    {
+      rate.sleep();
+      success = rate.cycleTime().toSec() < timeout;
+    }
+    return success;
+  }
+
+  bool reachGrasp (float& x, float& y, float& z , std::string& object_name, double& x_depth, double& y_depth, double& z_depth) {
+     // Generate random cuboid
+    geometry_msgs::Pose object_pose;
+    double xmin = 0.5;
+    double xmax = 0.7;
+    double ymin = -0.25;
+    double ymax = 0.25;
+    double zmin = 0.2;
+    double zmax = 0.7;
+    rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax);
+
+    double cuboid_size_min = 0.01;
+    double cuboid_size_max = 0.03;
+    rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max);
+
+    visual_tools_->generateRandomCuboid(object_pose, x_depth, y_depth, z_depth, pose_bounds, cuboid_bounds);
+
+    object_pose.position.x = x;
+    object_pose.position.y = y;
+    object_pose.position.z = z;
+
+    visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_depth, z_depth, object_name, rviz_visual_tools::RED);
+    visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM);
+    visual_tools_->trigger();
+
+    bool success = true;
+    double timeout = 5;
+    ros::Rate rate(100);
+    
+    while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)){
+      rate.sleep();
+      success = rate.cycleTime().toSec() < timeout;
+    }
+
+    if (!success){
+      ROS_ERROR_NAMED(LOGNAME, "Something went wring in Cube construction ._.");
+      return false;
+    }
+
+    // -----------------------------------
+    // Generate grasp candidates
+    std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates;
+
+    // Configure the desired types of grasps
+    moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config =
+        moveit_grasps::TwoFingerGraspCandidateConfig();
+    grasp_generator_config.disableAll();
+    grasp_generator_config.enable_face_grasps_ = true;
+    grasp_generator_config.generate_y_axis_grasps_ = true;
+    grasp_generator_config.generate_x_axis_grasps_ = true;
+    grasp_generator_config.generate_z_axis_grasps_ = true;
+
+    grasp_generator_->setGraspCandidateConfig(grasp_generator_config);
+    if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), x_depth, y_depth,
+                                          z_depth, grasp_data_, grasp_candidates)){
+      ROS_ERROR_NAMED(LOGNAME, "Something went wrong in grasp generation");
+      return false;
+    }
+    
+    // --------------------------------------------
+    // Generating a seed state for filtering grasps
+    robot_state::RobotStatePtr seed_state =
+        std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState());
+    Eigen::Isometry3d eef_mount_grasp_pose =
+        visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse();
+    if (!getIKSolution(arm_jmg_, eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName()))
+    {
+      ROS_WARN_STREAM_NAMED(LOGNAME, "The ideal seed state is not reachable. Using start state as seed.");
+    }
+
+    // --------------------------------------------
+    // Filtering grasps
+    // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates)
+    bool filter_pregrasps = true;
+    if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, arm_jmg_, seed_state, filter_pregrasps,
+                                     object_name)){
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Filter grasps failed");
+      return false;
+    }
+    if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates)){
+      ROS_WARN_NAMED(LOGNAME, "Grasp filtering removed all grasps");
+      return false;
+    }
+    ROS_INFO_STREAM_NAMED(LOGNAME, "" << grasp_candidates.size() << " remain after filtering");
+
+    // Plan free-space approach, cartesian approach, lift and retreat trajectories
+    /*
+    moveit_grasps::GraspCandidatePtr selected_grasp_candidate;
+    moveit_msgs::MotionPlanResponse pre_approach_plan;
+    if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name)){
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions");
+      return false;
+    }
+    */
+    
+
+    return true;
+  }
+
+private:
+  // A shared node handle
+  ros::NodeHandle nh_;
+
+  // Tool for visualizing things in Rviz
+  moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
+
+  // MoveIt! Grasps
+  moveit_grasps::TwoFingerGraspGeneratorPtr grasp_generator_;
+
+  // Robot-specific data for generating grasps
+  moveit_grasps::TwoFingerGraspDataPtr grasp_data_;
+
+  // For planning approach and retreats
+  moveit_grasps::GraspPlannerPtr grasp_planner_;
+
+  // For selecting good grasps
+  moveit_grasps::TwoFingerGraspFilterPtr grasp_filter_;
+
+  // Shared planning scene (load once for everything)
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+
+  // Arm
+  const robot_model::JointModelGroup* arm_jmg_;
+
+  // Robot
+  robot_model::RobotModelPtr robot_model_;
+
+  // All the motion planning components
+  planning_pipeline::PlanningPipelinePtr planning_pipeline_;
+
+  // Choose which arm to use
+  std::string ee_group_name_;
+  std::string planning_group_name_;
+
+};  // end of class
+
+}  // namespace moveit_grasps
+
+int main(int argc, char* argv[])
+{
+  int num_tests = 1;
+
+  ros::init(argc, argv, "grasp_filter_demo");
+
+  // Allow the action server to recieve and send ros messages
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
+
+  // Seed random
+  srand(ros::Time::now().toSec());
+
+  // Benchmark time
+  ros::Time start_time;
+  start_time = ros::Time::now();
+
+  // Head of yaml
+
+
+  // Guess I need this
+  double x_depth = 0.2;
+  double y_depth = 0.4;
+  double z_depth = 0.4;
+  float dimension_x = 0.9;
+  float dimension_y = 0.9;
+  float dimension_z = 0; //1.2;
+
+  float resolution = 0.2; 
+
+  moveit_grasps::GraspPipeline tester;
+  std::string object_name = "pick_target";
+
+  for (float x = 0 - dimension_x; x <= 0 + dimension_x; x+= resolution){
+      for(float y = 0 - dimension_y; y <= 0 + dimension_y; y+= resolution){
+          for(float z = 0 - dimension_z; z <= 0 + dimension_z; z+= resolution){
+            //tester.reachGrasp(x,y,z, object_name, x_depth, y_depth, z_depth);
+          }
+          tester.reachGrasp(x,y,dimension_z, object_name, x_depth, y_depth, z_depth);
+      }
+  }
+
+  
+
+
+  // Benchmark time
+  double duration = (ros::Time::now() - start_time).toSec();
+
+  node["description"]["system"] = "Franka Emika 'Panda'";
+  node["description"]["resolution"] = resolution;
+  node["description"]["dimension"] = std::to_string(dimension_x) + " " + std::to_string(dimension_y) + " " + std::to_string(dimension_z) + " (m)";
+  node["description"]["timestamp"] = std::to_string(duration) + " sec";
+
+  std::ofstream file(ros::package::getPath("mtc") + "/maps/dummy.yaml");
+  file << node;
+
+
+  ROS_INFO_STREAM_NAMED("grasp_filter_demo", "Total time: " << duration << "\t" << num_tests);
+  std::cout << "Total time: " << duration << "\t" << num_tests << std::endl;
+
+  ros::Duration(1.0).sleep();  // let rviz markers finish publishing
+
+  return 0;
+}
diff --git a/src/mtc/src/grasp_planner.cpp b/src/mtc/src/grasp_planner.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4cb443cf218ca23386d93795f7726cf1c1b3c96f
--- /dev/null
+++ b/src/mtc/src/grasp_planner.cpp
@@ -0,0 +1,435 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
+*/
+
+// moveit_grasps
+#include "../include/grasp_planner.h"
+#include "../include/state_validity_callback.h"
+#include <moveit/robot_state/cartesian_interpolator.h>
+
+// Parameter loading
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+namespace moveit_grasps
+{
+constexpr char ENABLED_PARENT_NAME[] = "grasp_planner";  // for namespacing logging messages
+constexpr char ENABLED_SETTINGS_NAMESPACE[] = "moveit_grasps/planner";
+
+GraspPlanner::GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools)
+  : nh_("~"), visual_tools_(visual_tools)
+{
+  loadEnabledSettings();
+}
+
+bool GraspPlanner::planAllApproachLiftRetreat(
+    std::vector<GraspCandidatePtr>& grasp_candidates, const robot_state::RobotStatePtr& robot_state,
+    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::string& grasp_object_id)
+{
+  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
+  ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor));
+  return planAllApproachLiftRetreat(grasp_candidates, robot_state,
+                                    static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), grasp_object_id);
+}
+
+bool GraspPlanner::planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
+                                              const robot_state::RobotStatePtr& robot_state,
+                                              const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                              const std::string& grasp_object_id)
+{
+  ROS_INFO_STREAM_NAMED("grasp_planner", "Planning all remaining grasps with approach lift retreat cartesian path");
+
+  // For each remaining grasp, calculate entire approach, lift, and retreat path.
+  // Remove those that have no valid path
+  bool verbose_cartesian_filtering = isEnabled("verbose_cartesian_filtering");
+  std::size_t grasp_candidates_before_cartesian_path = grasp_candidates.size();
+
+  std::size_t count = 0;
+  for (std::vector<GraspCandidatePtr>::iterator grasp_it = grasp_candidates.begin(); grasp_it != grasp_candidates.end();)
+  {
+    if (!ros::ok())
+      return false;
+
+    if (isEnabled("verbose_cartesian_filtering"))
+    {
+      ROS_INFO_STREAM_NAMED("grasp_planner", "");
+      ROS_INFO_STREAM_NAMED("grasp_planner", "Attempting to plan cartesian grasp path #"
+                                                 << count++ << ". " << grasp_candidates.size() << " remaining.");
+    }
+
+    if (!planApproachLiftRetreat(*grasp_it, robot_state, planning_scene, verbose_cartesian_filtering, grasp_object_id))
+    {
+      ROS_INFO_STREAM_NAMED("grasp_planner", "Grasp candidate was unable to find valid cartesian waypoint path");
+
+      grasp_it = grasp_candidates.erase(grasp_it);  // not valid
+    }
+    else
+    {
+      ++grasp_it;  // move to next grasp
+    }
+
+    if (isEnabled("show_cartesian_waypoints"))
+    {
+      visual_tools_->deleteAllMarkers();
+      visual_tools_->trigger();
+    }
+  }
+
+  // Results
+  if (isEnabled("statistics_verbose"))
+  {
+    std::cout << std::endl;
+    std::cout << "-------------------------------------------------------" << std::endl;
+    std::cout << "Total grasp candidates: " << grasp_candidates_before_cartesian_path << std::endl;
+    std::cout << "Failed due to invalid cartesian path: "
+              << grasp_candidates_before_cartesian_path - grasp_candidates.size() << std::endl;
+    std::cout << "Remaining grasp candidates: " << grasp_candidates.size() << std::endl;
+    std::cout << "-------------------------------------------------------" << std::endl;
+    std::cout << std::endl;
+  }
+
+  // If no grasp candidates had valid paths, then we return false
+  if (grasp_candidates.size() == 0)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_planner", "No valid grasp plan possible");
+    return false;
+  }
+  return true;
+}
+
+bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate,
+                                           const robot_state::RobotStatePtr& robot_state,
+                                           const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+                                           bool verbose_cartesian_filtering, const std::string& grasp_object_id)
+{
+  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
+  ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor));
+  return planApproachLiftRetreat(grasp_candidate, robot_state,
+                                 static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls),
+                                 verbose_cartesian_filtering, grasp_object_id);
+}
+
+bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate,
+                                           const robot_state::RobotStatePtr& robot_state,
+                                           const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                           bool verbose_cartesian_filtering, const std::string& grasp_object_id)
+{
+  EigenSTL::vector_Isometry3d waypoints;
+  GraspGenerator::getGraspWaypoints(grasp_candidate, waypoints);
+
+  // Visualize waypoints
+  bool show_cartesian_waypoints = isEnabled("show_cartesian_waypoints");
+  if (show_cartesian_waypoints)
+  {
+    visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp");
+    visual_tools_->publishAxisLabeled(waypoints[1], "grasp");
+    visual_tools_->publishAxisLabeled(waypoints[2], "lifted");
+    visual_tools_->publishAxisLabeled(waypoints[3], "retreat");
+    visual_tools_->trigger();
+
+    if (grasp_candidate->getPreGraspState(visual_tools_->getSharedRobotState()))
+    {
+      waitForNextStep("see pre grasp state");
+      visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::RED);
+      visual_tools_->trigger();
+    }
+    if (grasp_candidate->getGraspStateClosed(visual_tools_->getSharedRobotState()))
+    {
+      waitForNextStep("see grasp state");
+      visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::BLUE);
+      visual_tools_->trigger();
+    }
+    waitForNextStep("continue cartesian planning");
+  }
+
+  // Remove the pre-grasp point since we plan from that start state
+  waypoints.erase(waypoints.begin());
+
+  // Starting state
+  moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(*robot_state));
+  if (!grasp_candidate->getPreGraspState(start_state))
+  {
+    ROS_WARN_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp");
+    return false;
+  }
+
+  if (!computeCartesianWaypointPath(grasp_candidate, planning_scene, start_state, waypoints, grasp_object_id))
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Unable to plan approach lift retreat path");
+
+    return false;
+  }
+
+  // Feedback
+  ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid and complete waypoint manipulation path for grasp "
+                                                    "candidate");
+
+  // Show visuals
+  if (show_cartesian_waypoints)
+  {
+    ROS_INFO_STREAM_NAMED("grasp_planner.waypoints", "Visualize end effector position of cartesian path for "
+                                                         << grasp_candidate->segmented_cartesian_traj_.size()
+                                                         << " segments");
+    visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[APPROACH],
+                                           grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::YELLOW);
+    visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[LIFT],
+                                           grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::ORANGE);
+    visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[RETREAT],
+                                           grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::RED);
+    visual_tools_->trigger();
+
+    bool wait_for_animation = true;
+    visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[APPROACH],
+                                         grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+    visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[LIFT],
+                                         grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+    visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[RETREAT],
+                                         grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation);
+  }
+
+  if (verbose_cartesian_filtering)
+    waitForNextStep("try next candidate grasp");
+
+  return true;
+}
+
+bool GraspPlanner::computeCartesianWaypointPath(
+    GraspCandidatePtr& grasp_candidate, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+    const moveit::core::RobotStatePtr& start_state, const EigenSTL::vector_Isometry3d& waypoints,
+    const std::string& grasp_object_id)
+{
+  boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
+  ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor));
+
+  return computeCartesianWaypointPath(grasp_candidate, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls),
+                                      start_state, waypoints, grasp_object_id);
+}
+
+bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
+                                                const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                                const moveit::core::RobotStatePtr& start_state,
+                                                const EigenSTL::vector_Isometry3d& waypoints,
+                                                const std::string& grasp_object_id)
+{
+  // End effector parent link (arm tip for ik solving)
+  const moveit::core::LinkModel* ik_tip_link = grasp_candidate->grasp_data_->parent_link_;
+
+  // Resolution of trajectory
+  // The maximum distance in Cartesian space between consecutive points on the resulting path
+  const double max_step = 0.01;
+
+  // Jump threshold for preventing consequtive joint values from 'jumping' by a large amount in joint space
+  const double jump_threshold = 4;  // config_->jump_threshold_; // aka jump factor
+
+  // Collision setting
+  const bool collision_checking_verbose = isEnabled("collision_checking_verbose");
+  const bool only_check_self_collision = false;
+
+  // Reference frame setting
+  const bool global_reference_frame = true;
+
+  // Check for kinematic solver
+  if (!grasp_candidate->grasp_data_->arm_jmg_->canSetStateFromIK(ik_tip_link->getName()))
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "No IK Solver loaded - make sure moveit_config/kinamatics.yaml "
+                                                      "is loaded in this namespace");
+    return false;
+  }
+
+  std::size_t attempts = 0;
+  static const std::size_t MAX_IK_ATTEMPTS = 5;
+  bool valid_path_found = false;
+  while (attempts < MAX_IK_ATTEMPTS)
+  {
+    if (attempts > 0)
+    {
+      ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Attempting IK solution, attempt # " << attempts + 1);
+    }
+    attempts++;
+
+    moveit::core::RobotStatePtr start_state_copy(new moveit::core::RobotState(*start_state));
+    if (!grasp_candidate->getPreGraspState(start_state_copy))
+    {
+      ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp");
+      return false;
+    }
+
+    // Create a clone where we modify the ACM to disable collision checking between the end effector and object
+    planning_scene::PlanningScenePtr scene = planning_scene::PlanningScene::clone(planning_scene);
+    // If the grasp_object_id is set then we disable collision checking in the planning scene clone
+    if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id))
+    {
+      std::vector<std::string> ee_link_names = grasp_candidate->grasp_data_->ee_jmg_->getLinkModelNames();
+      for (const auto& ee_link : ee_link_names)
+        scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_id, ee_link, true);
+    }
+    // Collision check
+    moveit::core::GroupStateValidityCallbackFn constraint_fn =
+        boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision,
+                    visual_tools_, _1, _2, _3);
+
+    // Compute Cartesian Path
+    grasp_candidate->segmented_cartesian_traj_.clear();
+    grasp_candidate->segmented_cartesian_traj_.resize(3);
+    double valid_approach_percentage = robot_state::CartesianInterpolator::computeCartesianPath(
+        start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_,
+        grasp_candidate->segmented_cartesian_traj_[APPROACH], ik_tip_link, waypoints[APPROACH], global_reference_frame,
+        robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn,
+        kinematics::KinematicsQueryOptions());
+
+    if (!grasp_candidate->getGraspStateClosedEEOnly(start_state_copy))
+    {
+      ROS_ERROR_STREAM_NAMED("grasp_planner", "Unable to set pregrasp");
+      return false;
+    }
+
+    // Attach CO to end effector
+    if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id))
+    {
+      // Create an AttachedCollisionObject
+      moveit_msgs::AttachedCollisionObject aco;
+
+      // Create a reference to the collision object for convenience
+      moveit_msgs::CollisionObject& suction_voxel_co = aco.object;
+
+      suction_voxel_co.id = grasp_object_id;
+      suction_voxel_co.header.frame_id = ik_tip_link->getName();
+
+      // Set the aco attached link name
+      aco.link_name = ik_tip_link->getName();
+
+      // move the robot in the ps to the end of the approach path.
+      scene->setCurrentState(*start_state_copy);
+
+      // Mark object to be added
+      suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD;
+      if (!scene->processAttachedCollisionObjectMsg(aco))
+      {
+        ROS_WARN_STREAM_NAMED("grasp_planner", "Failed to attach: " << aco.object.id);
+      }
+      else
+      {
+        auto start_state_with_body = std::make_shared<robot_state::RobotState>(scene->getCurrentState());
+        std::vector<const robot_state::AttachedBody*> attached_bodies;
+        start_state_with_body->getAttachedBodies(attached_bodies);
+        for (const auto& ab : attached_bodies)
+          start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(),
+                                       ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(),
+                                       ab->getSubframes());
+        constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose,
+                                    only_check_self_collision, visual_tools_, _1, _2, _3);
+      }
+    }
+
+    double valid_lift_retreat_percentage = robot_state::CartesianInterpolator::computeCartesianPath(
+        start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_,
+        grasp_candidate->segmented_cartesian_traj_[LIFT], ik_tip_link, waypoints[LIFT], global_reference_frame,
+        robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn,
+        kinematics::KinematicsQueryOptions());
+
+    valid_lift_retreat_percentage *= robot_state::CartesianInterpolator::computeCartesianPath(
+        start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_,
+        grasp_candidate->segmented_cartesian_traj_[RETREAT], ik_tip_link, waypoints[RETREAT], global_reference_frame,
+        robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn,
+        kinematics::KinematicsQueryOptions());
+
+    ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "valid_approach_percentage: " << valid_approach_percentage
+                                                                                    << " \tvalid_lift_retreat_"
+                                                                                       "percentage: "
+                                                                                    << valid_lift_retreat_percentage);
+
+    // The retreat has to work for the most part but doesn't need to be perfect
+    double min_allowed_valid_lift_retreat_percentage = 0.90;
+    if (valid_approach_percentage == 1 && valid_lift_retreat_percentage >= min_allowed_valid_lift_retreat_percentage)
+    {
+      ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid cartesian path");
+      valid_path_found = true;
+      break;
+    }
+  }  // end while AND scoped pointer of locked planning scenep
+
+  if (!valid_path_found)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints",
+                           "UNABLE to find valid waypoint cartesian path after " << MAX_IK_ATTEMPTS << " attempts");
+    return false;
+  }
+
+  return true;
+}
+
+void GraspPlanner::waitForNextStep(const std::string& message)
+{
+  if (wait_for_next_step_callback_)
+    wait_for_next_step_callback_(message);
+}
+
+void GraspPlanner::setWaitForNextStepCallback(WaitForNextStepCallback callback)
+{
+  wait_for_next_step_callback_ = callback;
+}
+
+bool GraspPlanner::loadEnabledSettings()
+{
+  // Check if the map has been loaded yet
+  if (!enabled_settings_loaded_)
+  {
+    enabled_settings_loaded_ = true;
+    return rosparam_shortcuts::get(ENABLED_PARENT_NAME, nh_, ENABLED_SETTINGS_NAMESPACE, enabled_setting_);
+  }
+  return true;
+}
+
+bool GraspPlanner::isEnabled(const std::string& setting_name)
+{
+  // Check if the map has been loaded yet. it is preferred if this is called manually
+  if (!enabled_settings_loaded_)
+    ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "Enabled settings are not yet loaded e.g. call loadEnabledSettings()");
+
+  std::map<std::string, bool>::iterator it = enabled_setting_.find(setting_name);
+  if (it != enabled_setting_.end())
+  {
+    // Element found;
+    return it->second;
+  }
+  ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "isEnabled() key '" << nh_.getNamespace() << "/"
+                                                                   << ENABLED_SETTINGS_NAMESPACE << "/" << setting_name
+                                                                   << "' does not exist on the parameter server");
+
+  return false;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/grasp_scorer.cpp b/src/mtc/src/grasp_scorer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..934e4832e61a7d44a12006b281cc4aaece2d255c
--- /dev/null
+++ b/src/mtc/src/grasp_scorer.cpp
@@ -0,0 +1,165 @@
+/*********************************************************************
+ * Software License Agreement ("Modified BSD License")
+ *
+ * Copyright (c) 2014, University of Colorado, Boulder
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Univ of CO, Boulder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**
+ * Authors : Andy McEvoy ( mcevoy.andy@gmail.com )
+ * Desc    : Functions for scoring grasps. See *.h file for documentation
+ */
+
+#include "../include/grasp_scorer.h"
+
+namespace moveit_grasps
+{
+double GraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores,
+                                       const Eigen::Vector3d& translation_scores, bool verbose) const
+{
+  double total_score =
+      orientation_scores[0] * orientation_x_score_weight_ + orientation_scores[1] * orientation_y_score_weight_ +
+      orientation_scores[2] * orientation_z_score_weight_ + translation_scores[0] * translation_x_score_weight_ +
+      translation_scores[1] * translation_y_score_weight_ + translation_scores[2] * translation_z_score_weight_;
+
+  total_score /= getWeightTotal();
+
+  if (verbose)
+  {
+    static const std::string logger_name = "grasp_scorer.compute_score";
+    ROS_DEBUG_STREAM_NAMED(logger_name, "Grasp score: ");
+    // clang-format off
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_);
+    // Total
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score);
+    // clang-format on
+  }
+  return total_score;
+}
+
+double GraspScoreWeights::getWeightTotal() const
+{
+  return orientation_x_score_weight_ + orientation_y_score_weight_ + orientation_z_score_weight_ +
+         translation_x_score_weight_ + translation_y_score_weight_ + translation_z_score_weight_;
+}
+
+Eigen::Vector3d GraspScorer::scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                       const Eigen::Isometry3d& ideal_pose)
+{
+  Eigen::Vector3d grasp_pose_axis;
+  Eigen::Vector3d ideal_pose_axis;
+  Eigen::Vector3d scores;
+  double cos_angle;
+  double angle;
+
+  // get angle between x-axes
+  grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX();
+  ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitX();
+  cos_angle = grasp_pose_axis.dot(ideal_pose_axis);
+  angle = acos(std::max(-1.0, std::min(1.0, cos_angle)));
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "x angle = " << angle * 180.0 / M_PI);
+  scores[0] = (M_PI - angle) / M_PI;
+
+  // get angle between y-axes
+  grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY();
+  ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitY();
+  cos_angle = grasp_pose_axis.dot(ideal_pose_axis);
+  angle = acos(std::max(-1.0, std::min(1.0, cos_angle)));
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "y angle = " << angle * 180.0 / M_PI);
+  scores[1] = (M_PI - angle) / M_PI;
+
+  // get angle between z-axes
+  grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ();
+  ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitZ();
+  cos_angle = grasp_pose_axis.dot(ideal_pose_axis);
+  angle = acos(std::max(-1.0, std::min(1.0, cos_angle)));
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "z angle = " << angle * 180.0 / M_PI);
+  scores[2] = (M_PI - angle) / M_PI;
+
+  return scores;
+}
+
+Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                   const Eigen::Isometry3d& ideal_pose)
+{
+  // We assume that the ideal is in the middle
+  Eigen::Vector3d scores = -Eigen::Vector3d(grasp_pose_tcp.translation() - ideal_pose.translation()).array().abs();
+
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.scoreGraspTranslation",
+                         "value, ideal, score:\n"
+                             << "x: " << grasp_pose_tcp.translation()[0] << "\t" << ideal_pose.translation()[0] << "\t"
+                             << scores[0] << "\n"
+                             << "y: " << grasp_pose_tcp.translation()[1] << "\t" << ideal_pose.translation()[1] << "\t"
+                             << scores[1] << "\n"
+                             << "x: " << grasp_pose_tcp.translation()[2] << "\t" << ideal_pose.translation()[2] << "\t"
+                             << scores[2] << "\n");
+
+  return scores;
+}
+
+Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                   const Eigen::Vector3d& min_translations,
+                                                   const Eigen::Vector3d& max_translations)
+{
+  Eigen::Vector3d scores;
+
+  for (std::size_t i = 0; i < 3; ++i)
+  {
+    // We assume that the ideal is in the middle
+    double ideal = (max_translations[i] + min_translations[i]) / 2;
+    double translation = grasp_pose_tcp.translation()[i] - ideal;
+    double range = max_translations[i] - min_translations[i];
+    double score;
+    if (range == 0)
+      score = 0;
+    else
+      score = translation / range;
+
+    scores[i] = pow(score, 2);
+  }
+
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.translation",
+                         "\nvalue, min, max, score:\n"
+                             << grasp_pose_tcp.translation()[0] << ", " << min_translations[0] << ", "
+                             << max_translations[0] << ", " << scores[0] << "\n"
+                             << grasp_pose_tcp.translation()[1] << ", " << min_translations[1] << ", "
+                             << max_translations[1] << ", " << scores[1] << "\n"
+                             << grasp_pose_tcp.translation()[2] << ", " << min_translations[2] << ", "
+                             << max_translations[2] << ", " << scores[2] << "\n");
+
+  return scores;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/impl/base_by_rotation.cpp b/src/mtc/src/impl/base_by_rotation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..495a480d77a9b124c2721bba0adba1bfbbbc6868
--- /dev/null
+++ b/src/mtc/src/impl/base_by_rotation.cpp
@@ -0,0 +1,5 @@
+#include "impl/base_by_rotation.h"
+
+void Base_by_rotation::base_algorithm(){
+    ROS_INFO("hello");
+}
\ No newline at end of file
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1b5a3f560374ba32f0bcb76922a32d5c73ea59c
--- /dev/null
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -0,0 +1,57 @@
+#include <ros/ros.h>
+#include "impl/abstract_map_loader.h"
+#include "impl/map_loader.h"
+
+
+
+Map_loader::Map_loader(XmlRpc::XmlRpcValue& map, XmlRpc::XmlRpcValue& task){
+    std::regex reg("\\s+");
+    std::vector<geometry_msgs::Pose> inv_map;
+
+    ROS_INFO("setup inverse map calulation");
+    for (int i = 0; i < map.size();i++){
+        std::string str = map[i];
+        std::sregex_token_iterator to_it(str.begin(), str.end(),reg,-1);
+        std::vector<std::string> temp{to_it, {}};
+        tf2::Vector3 t(std::stof(temp[0]), std::stof(temp[1]), std::stof(temp[2]));
+        tf2::Quaternion so(std::stof(temp[3]), std::stof(temp[4]), std::stof(temp[5]), std::stof(temp[6]));
+
+        tf2::Transform trans(so, t);
+        trans = trans.inverse();
+        geometry_msgs::Pose pose;
+        pose.position.x = trans.getOrigin().getX();
+        pose.position.y = trans.getOrigin().getY();
+        pose.position.z = trans.getOrigin().getZ();
+        pose.orientation.x = trans.getRotation().getX();
+        pose.orientation.y = trans.getRotation().getY();
+        pose.orientation.z = trans.getRotation().getZ();
+        pose.orientation.w = trans.getRotation().getW();
+        inv_map.push_back(pose);
+    }
+    this->set_inv_map(inv_map);
+
+    std::vector<std::vector<geometry_msgs::Point>> task_;
+    for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = task.begin(); it != task.end(); ++it){
+        std::vector<geometry_msgs::Point> task_per_robot;
+        for (int i = 0; i < it->second.size(); i++){
+            double x = it->second[i][0];
+            double y = it->second[i][1];
+            double z = it->second[i][2];
+            geometry_msgs::Point point;
+            point.x = x;
+            point.y = y;
+            point.z = z;
+
+            task_per_robot.push_back(point);
+        } 
+        task_.push_back(task_per_robot);
+    }
+
+    this->set_task(task_);
+
+    ROS_INFO("Map is loaded with value %li", inv_map.size());
+}
+
+void Map_loader::base_calculation(){
+    this->get_strategy()->base_algorithm();
+}
\ No newline at end of file
diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8c58f8091983555f7df02a7c89842e7ace60a34
--- /dev/null
+++ b/src/mtc/src/mtc2taskspace.cpp
@@ -0,0 +1,81 @@
+#include "../include/mtc2taskspace.h"
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "mtc2taskspace");
+    ros::NodeHandle n;
+
+    std::string path;
+    XmlRpc::XmlRpcValue resources;
+    n.getParam("/objects", resources);
+
+    std::vector<tf2::Vector3> robot_1;
+    std::vector<tf2::Vector3> robot_2;
+    std::vector<tf2::Vector3> robot_middle;
+    std::vector<tf2::Vector3>* robot_pointer = nullptr;
+
+    tf2::Vector3 pos(0,0,0);
+    // resources is a TypeArray
+    for (int i = 0; i < resources.size(); i++){
+      // resources[i] is a struct, which could be a location, ot the table itself, tricky
+      // every struct has 5 values, but not sorted, dont know why
+      XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = resources[i].begin();
+
+      for(; it != resources[i].end(); ++it){
+        if(it->first == "id"){
+          ROS_INFO("hey");
+          std::string str = it->second;
+          if(str[0] == 'A')  robot_pointer = &robot_1;
+          if(str[0] == 'B')  robot_pointer = &robot_1;
+          if(str[0] == 'C')  robot_pointer = &robot_1;
+          if(str[0] == 'D')  robot_pointer = &robot_1;
+          if(str[0] == 'E')  robot_pointer = &robot_middle;
+          if(str[0] == 'F')  robot_pointer = &robot_middle;
+          if(str[0] == 'G')  robot_pointer = &robot_2;
+          if(str[0] == 'H')  robot_pointer = &robot_2;
+          if(str[0] == 'I')  robot_pointer = &robot_2;
+          if(str[0] == 'J')  robot_pointer = &robot_2;
+        }
+      }
+
+      if (robot_pointer == nullptr) continue;
+      it = resources[i].begin();
+
+      for(; it != resources[i].end(); ++it){
+        if(it->first == "pos"){
+          XmlRpc::XmlRpcValue::ValueStruct::const_iterator it_1 = it->second.begin();
+          for(; it_1 != it->second.end(); ++it_1){
+            float t = (it_1->second.getType() == XmlRpc::XmlRpcValue::TypeString) ? std::stof(it_1->second) : (double) it_1->second;
+            if(it_1->first == "x") pos.setX(t);
+            if(it_1->first == "y") pos.setY(t);
+            if(it_1->first == "z") pos.setZ(t);
+          }
+          robot_pointer->push_back(pos);
+          pos.setZero();
+        }
+      }
+
+      robot_pointer == nullptr;
+    }
+
+    // lets make some result data
+    std::ofstream o(ros::package::getPath("mtc") + "/descriptions/dummy.yaml");
+    YAML::Node node;
+    for(tf2::Vector3& vec : robot_1){
+      std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()};
+      node["task"]["groups"]["robot_x"].push_back(arr);
+    }
+
+    for(tf2::Vector3& vec : robot_2){
+      std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()};
+      node["task"]["groups"]["robot_y"].push_back(arr);
+    }
+
+    for(tf2::Vector3& vec : robot_middle){
+      std::vector<float> arr = {(float)vec.getX(), (float) vec.getY(), (float)vec.getZ()};
+      node["task"]["groups"]["robot_x"].push_back(arr);
+      node["task"]["groups"]["robot_y"].push_back(arr);
+    }
+
+    o << node;
+    return 0;
+}
\ No newline at end of file
diff --git a/src/mtc/src/two_finger_grasp_data.cpp b/src/mtc/src/two_finger_grasp_data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2db2eb0d9fc5891f89069a42a6f5798917f034c
--- /dev/null
+++ b/src/mtc/src/two_finger_grasp_data.cpp
@@ -0,0 +1,215 @@
+/*
+ * Software License Agreement (Modified BSD License)
+ *
+ *  Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L.
+ *     nor the names of its contributors may be used to endorse or
+ *     promote products derived from this software without specific
+ *     prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Authors: Dave Coleman, Bence Magyar
+   Description: Data class used by the grasp generator.
+*/
+
+#include "../include/two_finger_grasp_data.h"
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <eigen_conversions/eigen_msg.h>
+
+// C++
+#include <cmath>
+
+// Parameter loading
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+// Pose conversion
+#include <rviz_visual_tools/rviz_visual_tools.h>
+
+namespace moveit_grasps
+{
+const std::string LOGNAME = "grasp_data.two_finger_gripper";
+
+TwoFingerGraspData::TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector,
+                                       const moveit::core::RobotModelConstPtr& robot_model)
+  : GraspData(nh, end_effector, robot_model)
+{
+}
+
+bool TwoFingerGraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector)
+{
+  if (!GraspData::loadGraspData(nh, end_effector))
+  {
+    ROS_ERROR_STREAM_NAMED(LOGNAME, "GraspData::loadGraspData failed");
+    return false;
+  }
+  // Load all other parameters
+  const std::string parent_name = "grasp_data";  // for namespacing logging messages
+  std::size_t error = 0;
+
+  // Search within the sub-namespace of this end effector name
+  ros::NodeHandle child_nh(nh, end_effector);
+
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "gripper_finger_width", gripper_finger_width_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "max_grasp_width", max_grasp_width_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "max_finger_width", max_finger_width_);
+  error += !rosparam_shortcuts::get(parent_name, child_nh, "min_finger_width", min_finger_width_);
+  rosparam_shortcuts::shutdownIfError(parent_name, error);
+
+  return true;
+}
+
+bool TwoFingerGraspData::setGraspWidth(double fraction_open, double min_finger_width,
+                                       trajectory_msgs::JointTrajectory& grasp_posture)
+{
+  if (fraction_open < 0 || fraction_open > 1)
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_data", "Invalid fraction passed in " << fraction_open);
+    return false;
+  }
+
+  // Ensure min_finger_width is not less than actual min finger width
+  double min_finger_width_adjusted = std::max(min_finger_width, min_finger_width_);
+
+  // Max width = max_finger_width_
+  // Min width = min_finger_width_adjusted
+  double distance_btw_fingers =
+      min_finger_width_adjusted + (max_finger_width_ - min_finger_width_adjusted) * fraction_open;
+  return fingerWidthToGraspPosture(distance_btw_fingers, grasp_posture);
+}
+
+bool TwoFingerGraspData::fingerWidthToGraspPosture(double distance_btw_fingers,
+                                                   trajectory_msgs::JointTrajectory& grasp_posture)
+{
+  // TODO(mlautman): Change this function to take in a method for translating joint values to grasp width
+  //       Currently this function simply interpolates between max open and max closed
+  ROS_DEBUG_STREAM_NAMED("grasp_data",
+                         "Setting grasp posture to have distance_between_fingers of " << distance_btw_fingers);
+
+  // Error check
+  if (distance_btw_fingers > max_finger_width_ + std::numeric_limits<double>::epsilon() ||
+      distance_btw_fingers < min_finger_width_ - std::numeric_limits<double>::epsilon())
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_data", "Requested " << distance_btw_fingers << " is beyond limits of "
+                                                      << min_finger_width_ << "," << max_finger_width_);
+    return false;
+  }
+
+  // NOTE: We assume a linear relationship between the actuated joint values and the distance between fingers.
+  //       This is probably incorrect but until we expose an interface for passing in a function to translate from
+  //       joint values to grasp width, it's the best we got...
+  // TODO(mlautman): Make it so that a user can pass in a function here.
+  const std::vector<std::string>& joint_names = pre_grasp_posture_.joint_names;
+  const std::vector<double>& grasp_pose = grasp_posture_.points[0].positions;
+  const std::vector<double>& pre_grasp_pose = pre_grasp_posture_.points[0].positions;
+  if (joint_names.size() != grasp_pose.size() || grasp_pose.size() != pre_grasp_pose.size())
+  {
+    ROS_ERROR_NAMED("grasp_data",
+                    "Mismatched vector sizes joint_names.size()=%zu, grasp_pose.size()=%zu, and "
+                    "pre_grasp_pose.size()=%zu",
+                    joint_names.size(), grasp_pose.size(), pre_grasp_pose.size());
+    return false;
+  }
+
+  std::vector<double> slope(joint_names.size());
+  std::vector<double> intercept(joint_names.size());
+  std::vector<double> joint_positions(joint_names.size());
+  for (std::size_t joint_index = 0; joint_index < joint_names.size(); joint_index++)
+  {
+    slope[joint_index] =
+        (max_finger_width_ - min_finger_width_) / (pre_grasp_pose[joint_index] - grasp_pose[joint_index]);
+    intercept[joint_index] = max_finger_width_ - slope[joint_index] * pre_grasp_pose[joint_index];
+
+    // Sanity check
+    ROS_ASSERT_MSG(intercept[joint_index] == min_finger_width_ - slope[joint_index] * grasp_pose[joint_index],
+                   "we got different y intercept!! %.3f and %.3f", intercept[joint_index],
+                   min_finger_width_ - slope[joint_index] * grasp_pose[joint_index]);
+
+    joint_positions[joint_index] = (distance_btw_fingers - intercept[joint_index]) / slope[joint_index];
+
+    ROS_DEBUG_NAMED("grasp_data", "Converted joint %s to position %.3f", joint_names[joint_index].c_str(),
+                    joint_positions[joint_index]);
+  }
+
+  return jointPositionsToGraspPosture(joint_positions, grasp_posture);
+}
+
+bool TwoFingerGraspData::jointPositionsToGraspPosture(const std::vector<double>& joint_positions,
+                                                      trajectory_msgs::JointTrajectory& grasp_posture)
+{
+  for (std::size_t joint_index = 0; joint_index < pre_grasp_posture_.joint_names.size(); joint_index++)
+  {
+    const moveit::core::JointModel* joint = robot_model_->getJointModel(pre_grasp_posture_.joint_names[joint_index]);
+    ROS_ASSERT_MSG(joint->getVariableBounds().size() > 0, "joint->getVariableBounds() is empty for %s",
+                   pre_grasp_posture_.joint_names[joint_index].c_str());
+    const moveit::core::VariableBounds& bound = joint->getVariableBounds()[0];
+
+    if (joint_positions[joint_index] > bound.max_position_ || joint_positions[joint_index] < bound.min_position_)
+    {
+      ROS_ERROR_STREAM_NAMED("grasp_data", "Requested joint " << pre_grasp_posture_.joint_names[joint_index].c_str()
+                                                              << "at index" << joint_index << " with value "
+                                                              << joint_positions[joint_index] << " is beyond limits of "
+                                                              << bound.min_position_ << ", " << bound.max_position_);
+      return false;
+    }
+  }
+
+  // Get default grasp posture
+  grasp_posture = grasp_posture_;
+
+  // Error check
+  if (joint_positions.size() != grasp_posture.points.front().positions.size())
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_data",
+                           "Not enough finger joints passed in: " << joint_positions.size() << " positions but expect "
+                                                                  << grasp_posture.points.front().positions.size());
+    return false;
+  }
+
+  // Set joint positions
+  grasp_posture.points.front().positions = joint_positions;
+
+  return true;
+}
+
+void TwoFingerGraspData::print()
+{
+  GraspData::print();
+
+  std::cout << "Finger Gripper Parameters: " << std::endl;
+  std::cout << "\tpre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl;
+  std::cout << "\tgrasp_posture_: \n" << grasp_posture_ << std::endl;
+  std::cout << "\tgripper_finger_width_: " << gripper_finger_width_ << std::endl;
+  std::cout << "\tmax_grasp_width_: " << max_grasp_width_ << std::endl;
+  std::cout << "\tmax_finger_width_: " << max_finger_width_ << std::endl;
+  std::cout << "\tmin_finger_width_: " << min_finger_width_ << std::endl;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/two_finger_grasp_filter.cpp b/src/mtc/src/two_finger_grasp_filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11d2376983c8368e135441d014e061f919c1f681
--- /dev/null
+++ b/src/mtc/src/two_finger_grasp_filter.cpp
@@ -0,0 +1,95 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>
+   Desc:   Filters grasps based on kinematic feasibility and collision
+*/
+
+// Parent Class
+#include "../include/state_validity_callback.h"
+
+// moveit_grasps
+#include "../include/two_finger_grasp_filter.h"
+
+// moveit
+#include <moveit/transforms/transforms.h>
+#include <moveit/collision_detection/collision_tools.h>
+
+namespace moveit_grasps
+{
+// Constructor
+TwoFingerGraspFilter::TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state,
+                                           const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools)
+  : GraspFilter(robot_state, visual_tools), name_("two_finger_grasp_filter")
+{
+}
+
+bool TwoFingerGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct)
+{
+  bool filer_results = GraspFilter::processCandidateGrasp(ik_thread_struct);
+  if (!filer_results)
+    return false;
+
+  // Helper pointer
+  GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id];
+
+  moveit::core::GroupStateValidityCallbackFn constraint_fn =
+      boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(),
+                  collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1,
+                  _2, _3);
+
+  // Check if IK solution for grasp pose is valid for fingers closed as well
+  return checkFingersClosedIK(grasp_candidate->grasp_ik_solution_, ik_thread_struct, grasp_candidate, constraint_fn);
+}
+
+bool TwoFingerGraspFilter::checkFingersClosedIK(std::vector<double>& ik_solution,
+                                                const IkThreadStructPtr& ik_thread_struct,
+                                                GraspCandidatePtr& grasp_candidate,
+                                                const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const
+{
+  // Set gripper position (how open the fingers are) to CLOSED
+  grasp_candidate->getGraspStateClosedEEOnly(ik_thread_struct->robot_state_);
+
+  // Check constraint function
+  if (!constraint_fn(ik_thread_struct->robot_state_.get(), grasp_candidate->grasp_data_->arm_jmg_, &ik_solution[0]))
+  {
+    ROS_WARN_STREAM_NAMED(name_ + ".superdebug", "Grasp filtered because in collision with fingers CLOSED");
+    grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK_CLOSED;
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/two_finger_grasp_generator.cpp b/src/mtc/src/two_finger_grasp_generator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..969967742f554dab2d52d00c465ed10a9688a85d
--- /dev/null
+++ b/src/mtc/src/two_finger_grasp_generator.cpp
@@ -0,0 +1,887 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2015, University of Colorado, Boulder
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Univ of CO, Boulder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
+   Desc:   Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
+*/
+
+#include "../include/two_finger_grasp_generator.h"
+#include "../include/grasp_filter.h"
+
+#include <rosparam_shortcuts/rosparam_shortcuts.h>
+
+namespace
+{
+void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width,
+                            double grasp_padding_on_approach)
+{
+  ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to "
+                                                << percent_open << " % open. Stats:"
+                                                << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach
+                                                << "\n object_width: \t " << object_width
+                                                << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach);
+}
+
+}  // namespace
+
+namespace moveit_grasps
+{
+TwoFingerGraspCandidateConfig::TwoFingerGraspCandidateConfig()
+  : enable_corner_grasps_(true)
+  , enable_face_grasps_(true)
+  , enable_variable_angle_grasps_(true)
+  , enable_edge_grasps_(true)
+  , generate_x_axis_grasps_(true)
+  , generate_y_axis_grasps_(true)
+  , generate_z_axis_grasps_(true)
+{
+}
+void TwoFingerGraspCandidateConfig::enableAllGraspTypes()
+{
+  enable_corner_grasps_ = true;
+  enable_face_grasps_ = true;
+  enable_variable_angle_grasps_ = true;
+  enable_edge_grasps_ = true;
+}
+void TwoFingerGraspCandidateConfig::enableAllGraspAxes()
+{
+  generate_x_axis_grasps_ = true;
+  generate_y_axis_grasps_ = true;
+  generate_z_axis_grasps_ = true;
+}
+void TwoFingerGraspCandidateConfig::enableAll()
+{
+  enableAllGraspTypes();
+  enableAllGraspAxes();
+}
+void TwoFingerGraspCandidateConfig::disableAllGraspTypes()
+{
+  enable_corner_grasps_ = false;
+  enable_face_grasps_ = false;
+  enable_variable_angle_grasps_ = false;
+  enable_edge_grasps_ = false;
+}
+void TwoFingerGraspCandidateConfig::disableAllGraspAxes()
+{
+  generate_x_axis_grasps_ = false;
+  generate_y_axis_grasps_ = false;
+  generate_z_axis_grasps_ = false;
+}
+void TwoFingerGraspCandidateConfig::disableAll()
+{
+  disableAllGraspTypes();
+  disableAllGraspAxes();
+}
+
+// Constructor
+TwoFingerGraspGenerator::TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools,
+                                                 bool verbose)
+  : GraspGenerator(visual_tools, verbose)
+{
+  auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>();
+  grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights);
+
+  grasp_candidate_config_ = TwoFingerGraspCandidateConfig();
+}
+
+void TwoFingerGraspGenerator::setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config)
+{
+  grasp_candidate_config_ = grasp_candidate_config;
+}
+
+bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width,
+                                             double height, const moveit_grasps::GraspDataPtr& grasp_data,
+                                             std::vector<GraspCandidatePtr>& grasp_candidates)
+{
+  auto two_finger_grasp_data = std::dynamic_pointer_cast<TwoFingerGraspData>(grasp_data);
+  if (!two_finger_grasp_data)
+  {
+    ROS_ERROR_STREAM_NAMED("grasp_generator",
+                           "grasp_data is not castable to TwoFingerGraspData. Make sure you are using "
+                           "the child class");
+    return false;
+  }
+  return generateGrasps(cuboid_pose, depth, width, height, two_finger_grasp_data, grasp_candidates);
+}
+
+bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width,
+                                             double height, const TwoFingerGraspDataPtr& grasp_data,
+                                             std::vector<GraspCandidatePtr>& grasp_candidates)
+{
+  bool result =
+      generateFingerGrasps(cuboid_pose, depth, width, height, grasp_data, grasp_candidates, grasp_candidate_config_);
+
+  if (result)
+    std::sort(grasp_candidates.begin(), grasp_candidates.end(), GraspFilter::compareGraspScores);
+
+  return result;
+}
+
+bool TwoFingerGraspGenerator::addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount,
+                                       const TwoFingerGraspDataPtr& grasp_data, const Eigen::Isometry3d& object_pose,
+                                       const Eigen::Vector3d& object_size, double object_width,
+                                       std::vector<GraspCandidatePtr>& grasp_candidates)
+{
+  // Transform the grasp pose eef mount to the tcp grasp pose
+  Eigen::Isometry3d grasp_pose_tcp = grasp_pose_eef_mount * grasp_data->tcp_to_eef_mount_.inverse();
+
+  // The new grasp
+  moveit_msgs::Grasp new_grasp;
+
+  // Approach and retreat - aligned with eef to grasp transform
+  // set pregrasp
+  moveit_msgs::GripperTranslation pre_grasp_approach;
+  new_grasp.pre_grasp_approach.direction.header.stamp = ros::Time::now();
+  new_grasp.pre_grasp_approach.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->approach_distance_desired_;
+  new_grasp.pre_grasp_approach.min_distance = 0;  // NOT IMPLEMENTED
+  new_grasp.pre_grasp_approach.direction.header.frame_id = grasp_data->parent_link_->getName();
+
+  Eigen::Vector3d grasp_approach_vector = -1 * grasp_data->tcp_to_eef_mount_.translation();
+  grasp_approach_vector = grasp_approach_vector / grasp_approach_vector.norm();
+
+  new_grasp.pre_grasp_approach.direction.vector.x = grasp_approach_vector.x();
+  new_grasp.pre_grasp_approach.direction.vector.y = grasp_approach_vector.y();
+  new_grasp.pre_grasp_approach.direction.vector.z = grasp_approach_vector.z();
+
+  // set postgrasp
+  moveit_msgs::GripperTranslation post_grasp_retreat;
+  new_grasp.post_grasp_retreat.direction.header.stamp = ros::Time::now();
+  new_grasp.post_grasp_retreat.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->retreat_distance_desired_;
+  new_grasp.post_grasp_retreat.min_distance = 0;  // NOT IMPLEMENTED
+  new_grasp.post_grasp_retreat.direction.header.frame_id = grasp_data->parent_link_->getName();
+  new_grasp.post_grasp_retreat.direction.vector.x = -1 * grasp_approach_vector.x();
+  new_grasp.post_grasp_retreat.direction.vector.y = -1 * grasp_approach_vector.y();
+  new_grasp.post_grasp_retreat.direction.vector.z = -1 * grasp_approach_vector.z();
+
+  // set grasp pose
+  geometry_msgs::PoseStamped grasp_pose_msg;
+  grasp_pose_msg.header.stamp = ros::Time::now();
+  grasp_pose_msg.header.frame_id = grasp_data->base_link_;
+
+  // name the grasp
+  static std::size_t grasp_id = 0;
+  new_grasp.id = "Grasp" + boost::lexical_cast<std::string>(grasp_id);
+  grasp_id++;
+
+  tf::poseEigenToMsg(grasp_pose_eef_mount, grasp_pose_msg.pose);
+  new_grasp.grasp_pose = grasp_pose_msg;
+
+  // set grasp postures e.g. hand closed
+  new_grasp.grasp_posture = grasp_data->grasp_posture_;
+
+  // set minimum opening of fingers for pre grasp approach
+  double min_finger_open_on_approach = object_width + 2 * grasp_data->grasp_padding_on_approach_;
+  double percent_open;
+
+  // Create grasp with widest fingers possible ----------------------------------------------
+  percent_open = 1.0;
+  if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture))
+  {
+    debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width,
+                           grasp_data->grasp_padding_on_approach_);
+    return false;
+  }
+
+  new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open);
+
+  // Show visualization for widest grasp
+
+  grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose));
+
+  // Create grasp with middle width fingers -------------------------------------------------
+  percent_open = 0.5;
+  if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture))
+  {
+    debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width,
+                           grasp_data->grasp_padding_on_approach_);
+    return false;
+  }
+  new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open);
+  grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose));
+
+  // Create grasp with fingers at minimum width ---------------------------------------------
+  percent_open = 0.0;
+  if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture))
+  {
+    debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width,
+                           grasp_data->grasp_padding_on_approach_);
+    return false;
+  }
+  new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open);
+  grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose));
+
+  return true;
+}
+
+bool TwoFingerGraspGenerator::generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width,
+                                                       double height, grasp_axis_t axis,
+                                                       const TwoFingerGraspDataPtr& grasp_data,
+                                                       const TwoFingerGraspCandidateConfig& grasp_candidate_config,
+                                                       std::vector<GraspCandidatePtr>& grasp_candidates)
+{
+  double finger_depth = grasp_data->grasp_max_depth_ - grasp_data->grasp_min_depth_;
+  double length_along_a, length_along_b, length_along_c;
+  double delta_a, delta_b, delta_f;
+  double alpha_x, alpha_y, alpha_z;
+  Eigen::Vector3d object_size(depth, width, height);
+
+  double object_width;
+  EigenSTL::vector_Isometry3d grasp_poses_tcp;
+
+  Eigen::Isometry3d grasp_pose_tcp = cuboid_pose;
+  Eigen::Vector3d a_dir, b_dir, c_dir;
+
+  if (axis == X_AXIS)
+  {
+    length_along_a = width;
+    length_along_b = height;
+    length_along_c = depth;
+    a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY();
+    b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ();
+    c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX();
+    alpha_x = -M_PI / 2.0;
+    alpha_y = 0;
+    alpha_z = -M_PI / 2.0;
+    object_width = depth;
+  }
+  else if (axis == Y_AXIS)
+  {
+    length_along_a = depth;
+    length_along_b = height;
+    length_along_c = width;
+    a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX();
+    b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ();
+    c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY();
+    alpha_x = 0;
+    alpha_y = M_PI / 2.0;
+    alpha_z = M_PI;
+    object_width = width;
+  }
+  else  // Z_AXIS
+  {
+    length_along_a = depth;
+    length_along_b = width;
+    length_along_c = height;
+    a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX();
+    b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY();
+    c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ();
+    alpha_x = M_PI / 2.0;
+    alpha_y = M_PI / 2.0;
+    alpha_z = 0;
+    object_width = height;
+  }
+
+  double rotation_angles[3];
+  rotation_angles[0] = alpha_x;
+  rotation_angles[1] = alpha_y;
+  rotation_angles[2] = alpha_z;
+
+  a_dir = a_dir.normalized();
+  b_dir = b_dir.normalized();
+  c_dir = c_dir.normalized();
+
+  // Add grasps at corners, grasps are centroid aligned
+  double offset = 0.001;  // back the palm off of the object slightly
+  Eigen::Vector3d corner_translation_a;
+  Eigen::Vector3d corner_translation_b;
+  double angle_res = grasp_data->angle_resolution_ * M_PI / 180.0;
+  std::size_t num_radial_grasps = ceil((M_PI / 2.0) / angle_res);
+  Eigen::Vector3d translation;
+
+  if (grasp_candidate_config.enable_corner_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding corner grasps...");
+    corner_translation_a = 0.5 * (length_along_a + offset) * a_dir;
+    corner_translation_b = 0.5 * (length_along_b + offset) * b_dir;
+
+    if (num_radial_grasps <= 0)
+      num_radial_grasps = 1;
+
+    // move to corner 0.5 * ( -a, -b)
+    translation = -corner_translation_a - corner_translation_b;
+    addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, 0.0, num_radial_grasps, grasp_poses_tcp);
+
+    // move to corner 0.5 * ( -a, +b)
+    translation = -corner_translation_a + corner_translation_b;
+    addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, -M_PI / 2.0, num_radial_grasps, grasp_poses_tcp);
+
+    // move to corner 0.5 * ( +a, +b)
+    translation = corner_translation_a + corner_translation_b;
+    addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI, num_radial_grasps, grasp_poses_tcp);
+
+    // move to corner 0.5 * ( +a, -b)
+    translation = corner_translation_a - corner_translation_b;
+    addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI / 2.0, num_radial_grasps, grasp_poses_tcp);
+  }
+  std::size_t num_corner_grasps = grasp_poses_tcp.size();
+
+  // Create grasps along faces of cuboid, grasps are axis aligned
+  std::size_t num_grasps_along_a;
+  std::size_t num_grasps_along_b;
+  double rotation;
+  Eigen::Vector3d a_translation;
+  Eigen::Vector3d b_translation;
+  Eigen::Vector3d delta;
+
+  // get exact deltas for sides from desired delta
+  num_grasps_along_a = floor((length_along_a - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1;
+  num_grasps_along_b = floor((length_along_b - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1;
+
+  // if the gripper fingers are wider than the object we're trying to grasp, try with gripper aligned with
+  // top/center/bottom of object
+  // note that current implementation limits objects that are the same size as the gripper_finger_width to 1 grasp
+  if (num_grasps_along_a <= 0)
+  {
+    delta_a = length_along_a - grasp_data->gripper_finger_width_ / 2.0;
+    num_grasps_along_a = 3;
+  }
+  if (num_grasps_along_b <= 0)
+  {
+    delta_b = length_along_b - grasp_data->gripper_finger_width_ / 2.0;
+    num_grasps_along_b = 3;
+  }
+
+  if (num_grasps_along_a == 1)
+    delta_a = 0;
+  else
+    delta_a = (length_along_a - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_a - 1);
+
+  if (num_grasps_along_b == 1)
+    delta_b = 0;
+  else
+    delta_b = (length_along_b - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_b - 1);
+
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "delta_a : delta_b = " << delta_a << " : " << delta_b);
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "num_grasps_along_a : num_grasps_along_b  = "
+                                                   << num_grasps_along_a << " : " << num_grasps_along_b);
+
+  // TODO(mlautman): There is a bug with face grasps allowing the grasp generator to generate grasps where the gripper
+  // fingers
+  //                 are in collision with the object being grasped
+  if (grasp_candidate_config.enable_face_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding face grasps...");
+
+    a_translation = -(0.5 * (length_along_a + offset) * a_dir) -
+                    0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir;
+    b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir -
+                    (0.5 * (length_along_b + offset) * b_dir);
+
+    // grasps along -a_dir face
+    delta = delta_b * b_dir;
+    rotation = 0.0;
+    addFaceGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp);
+
+    // grasps along +b_dir face
+    rotation = -M_PI / 2.0;
+    delta = -delta_a * a_dir;
+    addFaceGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp);
+
+    // grasps along +a_dir face
+    rotation = M_PI;
+    delta = -delta_b * b_dir;
+    addFaceGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp);
+
+    // grasps along -b_dir face
+    rotation = M_PI / 2.0;
+    delta = delta_a * a_dir;
+    addFaceGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp);
+  }
+
+  // add grasps at variable angles
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding variable angle grasps...");
+  Eigen::Isometry3d base_pose;
+  std::size_t num_grasps = grasp_poses_tcp.size();
+  if (grasp_candidate_config.enable_variable_angle_grasps_)
+  {
+    // corner grasps at zero depth don't need variable angles
+    for (std::size_t i = num_corner_grasps; i < num_grasps; ++i)
+    {
+      base_pose = grasp_poses_tcp[i];
+
+      grasp_pose_tcp = base_pose * Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY());
+      std::size_t max_iterations = M_PI / angle_res + 1;
+      std::size_t iterations = 0;
+      while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data))
+      {
+        grasp_poses_tcp.push_back(grasp_pose_tcp);
+        grasp_pose_tcp *= Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY());
+        iterations++;
+        if (iterations > max_iterations)
+        {
+          ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps");
+          break;
+        }
+      }
+
+      iterations = 0;
+      grasp_pose_tcp = base_pose * Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY());
+      while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data))
+      {
+        grasp_poses_tcp.push_back(grasp_pose_tcp);
+        // visual_tools_->publishZArrow(grasp_pose_tcp, rviz_visual_tools::CYAN, rviz_visual_tools::XSMALL, 0.02);
+        grasp_pose_tcp *= Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY());
+        // ros::Duration(0.2).sleep();
+        iterations++;
+        if (iterations > max_iterations)
+        {
+          ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps");
+          break;
+        }
+      }
+    }
+  }
+
+  if (grasp_candidate_config.enable_edge_grasps_)
+  {
+    // Add grasps along edges
+    // move grasp pose to edge of cuboid
+    double a_sign = 1.0;
+    double b_sign = 1.0;
+    double a_rot_sign = 1.0;
+    double b_rot_sign = 1.0;
+
+    if (axis == Y_AXIS)
+    {
+      a_sign = -1.0;
+      b_rot_sign = -1.0;
+    }
+
+    if (axis == Z_AXIS)
+    {
+      a_sign = -1.0;
+      b_sign = -1.0;
+      a_rot_sign = -1.0;
+      b_rot_sign = -1.0;
+    }
+
+    a_translation = -0.5 * (length_along_a + offset) * a_dir -
+                    0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir -
+                    0.5 * (length_along_c + offset) * c_dir * a_sign;
+    b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir -
+                    (0.5 * (length_along_b + offset) * b_dir) - 0.5 * (length_along_c + offset) * c_dir * b_sign;
+
+    // grasps along -a_dir face
+    delta = delta_b * b_dir;
+    rotation = 0.0;
+    addEdgeGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp, -M_PI / 4.0 * a_rot_sign);
+
+    // grasps along +b_dir face
+    rotation = -M_PI / 2.0;
+    delta = -delta_a * a_dir;
+    addEdgeGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp, M_PI / 4.0 * b_rot_sign);
+
+    // grasps along +a_dir face
+    rotation = M_PI;
+    delta = -delta_b * b_dir;
+    addEdgeGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp, M_PI / 4.0 * a_rot_sign);
+
+    // grasps along -b_dir face
+    rotation = M_PI / 2.0;
+    delta = delta_a * a_dir;
+    addEdgeGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b,
+                        grasp_poses_tcp, -M_PI / 4.0 * b_rot_sign);
+  }
+  // Add grasps at variable depths
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding depth grasps...");
+  std::size_t num_depth_grasps = ceil(finger_depth / grasp_data->grasp_depth_resolution_);
+  if (num_depth_grasps <= 0)
+    num_depth_grasps = 1;
+  delta_f = finger_depth / static_cast<double>(num_depth_grasps);
+
+  num_grasps = grasp_poses_tcp.size();
+  Eigen::Vector3d grasp_dir;
+  Eigen::Isometry3d depth_pose;
+
+  for (std::size_t i = 0; i < num_grasps; ++i)
+  {
+    grasp_dir = grasp_poses_tcp[i].rotation() * Eigen::Vector3d::UnitZ();
+    depth_pose = grasp_poses_tcp[i];
+    for (std::size_t j = 0; j < num_depth_grasps; ++j)
+    {
+      depth_pose.translation() += delta_f * grasp_dir;
+      grasp_poses_tcp.push_back(depth_pose);
+    }
+  }
+
+  // add grasps in both directions
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding bi-directional grasps...");
+  num_grasps = grasp_poses_tcp.size();
+  for (std::size_t i = 0; i < num_grasps; ++i)
+  {
+    grasp_pose_tcp = grasp_poses_tcp[i];
+    grasp_pose_tcp *= Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());
+    grasp_poses_tcp.push_back(grasp_pose_tcp);
+  }
+
+  // compute min/max distances to object
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "computing min/max grasp distance...");
+  num_grasps = grasp_poses_tcp.size();
+  min_grasp_distance_ = std::numeric_limits<double>::max();
+  max_grasp_distance_ = std::numeric_limits<double>::min();
+  min_translations_ = Eigen::Vector3d(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(),
+                                      std::numeric_limits<double>::max());
+  max_translations_ = Eigen::Vector3d(std::numeric_limits<double>::min(), std::numeric_limits<double>::min(),
+                                      std::numeric_limits<double>::min());
+  double grasp_distance;
+
+  for (std::size_t i = 0; i < num_grasps; ++i)
+  {
+    grasp_pose_tcp = grasp_poses_tcp[i];
+    grasp_distance = (grasp_pose_tcp.translation() - cuboid_pose.translation()).norm();
+    if (grasp_distance > max_grasp_distance_)
+      max_grasp_distance_ = grasp_distance;
+
+    if (grasp_distance < min_grasp_distance_)
+      min_grasp_distance_ = grasp_distance;
+
+    for (std::size_t j = 0; j < 3; ++j)
+    {
+      if (grasp_pose_tcp.translation()[j] < min_translations_[j])
+        min_translations_[j] = grasp_pose_tcp.translation()[j];
+
+      if (grasp_pose_tcp.translation()[j] > max_translations_[j])
+        max_translations_[j] = grasp_pose_tcp.translation()[j];
+    }
+  }
+
+  ROS_DEBUG_STREAM_NAMED("grasp_generator.add",
+                         "min/max distance = " << min_grasp_distance_ << ", " << max_grasp_distance_);
+
+  // add all poses as possible grasps
+  std::size_t num_grasps_added = 0;
+
+  for (std::size_t i = 0; i < grasp_poses_tcp.size(); ++i)
+  {
+    Eigen::Isometry3d grasp_pose_eef_mount = grasp_poses_tcp[i] * grasp_data->tcp_to_eef_mount_;
+    if (!addGrasp(grasp_pose_eef_mount, grasp_data, cuboid_pose, object_size, object_width, grasp_candidates))
+    {
+      ROS_DEBUG_STREAM_NAMED("grasp_generator.add", "Unable to add grasp - function returned false");
+    }
+    else
+      num_grasps_added++;
+  }
+  ROS_INFO_STREAM_NAMED("grasp_generator.add", "\033[1;36madded " << num_grasps_added << " of "
+                                                                  << grasp_poses_tcp.size()
+                                                                  << " grasp poses created\033[0m");
+  return true;
+}
+
+std::size_t TwoFingerGraspGenerator::addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
+                                                           const Eigen::Vector3d& translation, double corner_rotation,
+                                                           std::size_t num_radial_grasps,
+                                                           EigenSTL::vector_Isometry3d& grasp_poses_tcp)
+{
+  std::size_t num_grasps_added = 0;
+  double delta_angle = (M_PI / 2.0) / static_cast<double>(num_radial_grasps + 1);
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta_angle = " << delta_angle);
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_radial_grasps = " << num_radial_grasps);
+
+  // rotate & translate pose to be aligned with edge of cuboid
+  Eigen::Isometry3d grasp_pose_tcp = pose;
+  grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) *
+                    Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) *
+                    Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ());
+  grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitY());
+  grasp_pose_tcp.translation() += translation;
+
+  for (std::size_t i = 0; i < num_radial_grasps; ++i)
+  {
+    // Eigen::Vector3d grasp_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ();
+    // Eigen::Isometry3d radial_pose = grasp_pose_tcp;
+    grasp_pose_tcp *= Eigen::AngleAxisd(delta_angle, Eigen::Vector3d::UnitY());
+    grasp_poses_tcp.push_back(grasp_pose_tcp);
+    num_grasps_added++;
+  }
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = "
+                                                          << num_grasps_added << " : " << grasp_poses_tcp.size());
+  return num_grasps_added;
+}
+
+std::size_t TwoFingerGraspGenerator::addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
+                                                         const Eigen::Vector3d& translation,
+                                                         const Eigen::Vector3d& delta, double alignment_rotation,
+                                                         std::size_t num_grasps,
+                                                         EigenSTL::vector_Isometry3d& grasp_poses_tcp)
+{
+  std::size_t num_grasps_added = 0;
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta);
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps);
+
+  Eigen::Isometry3d grasp_pose_tcp = pose;
+  grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) *
+                    Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) *
+                    Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ());
+  grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY());
+  grasp_pose_tcp.translation() += translation;
+
+  for (std::size_t i = 0; i < num_grasps; ++i)
+  {
+    grasp_pose_tcp.translation() += delta;
+    grasp_poses_tcp.push_back(grasp_pose_tcp);
+    num_grasps_added++;
+  }
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = "
+                                                          << num_grasps_added << " : " << grasp_poses_tcp.size());
+  return true;
+}
+
+std::size_t TwoFingerGraspGenerator::addEdgeGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
+                                                         const Eigen::Vector3d& translation,
+                                                         const Eigen::Vector3d& delta, double alignment_rotation,
+                                                         std::size_t num_grasps,
+                                                         EigenSTL::vector_Isometry3d& grasp_poses_tcp,
+                                                         double corner_rotation)
+{
+  std::size_t num_grasps_added = 0;
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta);
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps);
+
+  Eigen::Isometry3d grasp_pose_tcp = pose;
+  grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) *
+                    Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) *
+                    Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ());
+  grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY());
+
+  // rotate towards cuboid
+  grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitX());
+  grasp_pose_tcp.translation() += translation;
+
+  for (std::size_t i = 0; i < num_grasps; ++i)
+  {
+    grasp_pose_tcp.translation() += delta;
+    grasp_poses_tcp.push_back(grasp_pose_tcp);
+    num_grasps_added++;
+  }
+  ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = "
+                                                          << num_grasps_added << " : " << grasp_poses_tcp.size());
+  return true;
+}
+
+bool TwoFingerGraspGenerator::graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width,
+                                                      double height, const Eigen::Isometry3d& grasp_pose_tcp,
+                                                      const TwoFingerGraspDataPtr& grasp_data)
+{
+  // TODO(davetcoleman): add parameter to enable vizualization commented lines after further testing
+
+  // get line segment from grasp point to fingertip
+  Eigen::Vector3d point_a = grasp_pose_tcp.translation();
+  Eigen::Vector3d point_b =
+      point_a + grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ() * grasp_data->grasp_max_depth_;
+
+  // translate points into cuboid coordinate system
+  point_a = cuboid_pose.inverse() * point_a;  // T_cuboid-world * p_world = p_cuboid
+  point_b = cuboid_pose.inverse() * point_b;
+
+  double t, u, v;
+  Eigen::Vector3d intersection;
+  // check if line segment intersects XY faces of cuboid (z = +/- height/2)
+  t = (height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]);  // parameterization of line segment in 3d
+  if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v))
+  {
+    return true;
+  }
+
+  t = (-height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]);
+  if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v))
+  {
+    return true;
+  }
+
+  // check if line segment intersects XZ faces of cuboid (y = +/- width/2)
+  t = (width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]);
+  if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v))
+  {
+    return true;
+  }
+
+  t = (-width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]);
+  if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v))
+  {
+    return true;
+  }
+
+  // check if line segment intersects YZ faces of cuboid (x = +/- depth/2)
+  t = (depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]);
+  if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v))
+  {
+    return true;
+  }
+
+  t = (-depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]);
+  if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v))
+  {
+    return true;
+  }
+
+  // no intersection found
+  return false;
+}
+
+bool TwoFingerGraspGenerator::intersectionHelper(double t, double u1, double v1, double u2, double v2, double a,
+                                                 double b, double& u, double& v)
+{
+  // plane must cross through our line segment
+  if (t >= 0 && t <= 1)
+  {
+    u = u1 + t * (u2 - u1);
+    v = v1 + t * (v2 - v1);
+
+    if (u >= -a / 2 && u <= a / 2 && v >= -b / 2 && v <= b / 2)
+      return true;
+  }
+
+  return false;
+}
+
+double TwoFingerGraspGenerator::scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                 const TwoFingerGraspDataPtr& grasp_data,
+                                                 const Eigen::Isometry3d& object_pose, double percent_open)
+{
+  static const std::string logger_name = "grasp_generator.scoreGrasp";
+  ROS_DEBUG_STREAM_NAMED(logger_name, "starting to score grasp...");
+
+  // get portion of score based on the gripper's opening width on approach
+  double width_score = TwoFingerGraspScorer::scoreGraspWidth(grasp_data, percent_open);
+
+  // get portion of score based on the pinchers being down
+  Eigen::Vector3d orientation_scores =
+      TwoFingerGraspScorer::scoreRotationsFromDesired(grasp_pose_tcp, ideal_grasp_pose_);
+
+  // get portion of score based on the distance of the grasp pose to the object pose
+
+  // NOTE: when this function is called we've lost the references to the acutal size of the object.
+  // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object
+  // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an
+  // arbitrary number given our target object set
+  double distance_score = TwoFingerGraspScorer::scoreDistanceToPalm(grasp_pose_tcp, grasp_data, object_pose,
+                                                                    min_grasp_distance_, max_grasp_distance_);
+
+  // should really change this to be like orienation_scores so we can score any translation
+  Eigen::Vector3d translation_scores =
+      TwoFingerGraspScorer::scoreGraspTranslation(grasp_pose_tcp, min_translations_, max_translations_);
+
+  // want minimum translation
+  translation_scores *= -1.0;
+  translation_scores += Eigen::Vector3d(1.0, 1.0, 1.0);
+
+  double total_score = 0;
+  auto two_finger_grasp_score_weights = std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_);
+  if (two_finger_grasp_score_weights)
+  {
+    total_score = two_finger_grasp_score_weights->computeScore(orientation_scores, translation_scores, distance_score,
+                                                               width_score, getVerbose());
+  }
+  else
+  {
+    ROS_WARN_NAMED(logger_name, "Failed to cast grasp_score_weights_ as TwoFingerGraspScoreWeights. continuing without "
+                                "finger specific scores");
+    total_score = grasp_score_weights_->computeScore(orientation_scores, translation_scores, getVerbose());
+  }
+
+  return total_score;
+}
+
+bool TwoFingerGraspGenerator::generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width,
+                                                   double height, const TwoFingerGraspDataPtr& grasp_data,
+                                                   std::vector<GraspCandidatePtr>& grasp_candidates,
+                                                   const TwoFingerGraspCandidateConfig& grasp_candidate_config)
+{
+  // Generate grasps over axes that aren't too wide to grip
+  // Most default type of grasp is X axis
+  TwoFingerGraspCandidateConfig grasp_candidate_config_copy(grasp_candidate_config);
+  if (grasp_candidate_config_copy.generate_x_axis_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around x-axis of cuboid");
+    if (depth > grasp_data->max_grasp_width_)  // depth = size along x-axis
+    {
+      grasp_candidate_config_copy.disableAllGraspTypes();
+      grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
+      grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
+    }
+    generateCuboidAxisGrasps(cuboid_pose, depth, width, height, X_AXIS, grasp_data, grasp_candidate_config_copy,
+                             grasp_candidates);
+  }
+
+  grasp_candidate_config_copy = grasp_candidate_config;
+  if (grasp_candidate_config_copy.generate_y_axis_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around y-axis of cuboid");
+    if (width > grasp_data->max_grasp_width_)  // width = size along y-axis
+    {
+      grasp_candidate_config_copy.disableAllGraspTypes();
+      grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
+      grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
+    }
+    generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Y_AXIS, grasp_data, grasp_candidate_config_copy,
+                             grasp_candidates);
+  }
+
+  grasp_candidate_config_copy = grasp_candidate_config;
+  if (grasp_candidate_config_copy.generate_z_axis_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around z-axis of cuboid");
+    if (height > grasp_data->max_grasp_width_)  // height = size along z-axis
+    {
+      grasp_candidate_config_copy.disableAllGraspTypes();
+      grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_;
+      grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_;
+    }
+    generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Z_AXIS, grasp_data, grasp_candidate_config_copy,
+                             grasp_candidates);
+  }
+
+  if (!grasp_candidates.size())
+    ROS_WARN_STREAM_NAMED("grasp_generator", "Generated 0 grasps");
+  else
+    ROS_INFO_STREAM_NAMED("grasp_generator", "Generated " << grasp_candidates.size() << " grasps");
+
+  // Visualize animated grasps that have been generated
+  if (show_prefiltered_grasps_)
+  {
+    ROS_DEBUG_STREAM_NAMED("grasp_generator", "Animating all generated (candidate) grasps before filtering");
+    visualizeAnimatedGrasps(grasp_candidates, grasp_data->ee_jmg_, show_prefiltered_grasps_speed_);
+  }
+
+  return true;
+}
+
+}  // namespace moveit_grasps
diff --git a/src/mtc/src/two_finger_grasp_scorer.cpp b/src/mtc/src/two_finger_grasp_scorer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c542176fc239759108533e31b4a939b6e3a5269e
--- /dev/null
+++ b/src/mtc/src/two_finger_grasp_scorer.cpp
@@ -0,0 +1,102 @@
+/*********************************************************************
+ * Software License Agreement ("Modified BSD License")
+ *
+ * Copyright (c) 2014, University of Colorado, Boulder
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Univ of CO, Boulder nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/**
+ * Authors : Andy McEvoy ( mcevoy.andy@gmail.com )
+ * Desc    : Functions for scoring grasps. See *.h file for documentation
+ */
+
+#include "../include/two_finger_grasp_scorer.h"
+
+namespace moveit_grasps
+{
+double TwoFingerGraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores,
+                                                const Eigen::Vector3d& translation_scores, double depth_score,
+                                                double width_score, bool verbose) const
+{
+  double total_score = GraspScoreWeights::computeScore(orientation_scores, translation_scores, false) *
+                       GraspScoreWeights::getWeightTotal();
+  total_score += depth_score * depth_score_weight_ + width_score * width_score_weight_;
+
+  total_score /= getWeightTotal();
+
+  if (verbose)
+  {
+    static const std::string logger_name = "grasp_scorer.compute_score";
+    // clang-format off
+    ROS_DEBUG_STREAM_NAMED(logger_name, "Two Finger Grasp score: ");
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\tdepth_score         = " << depth_score           << "\tweight = "<< depth_score_weight_);
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\twidth_score         = " << width_score           << "\tweight = "<< width_score_weight_);
+    // Total
+    ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score);
+    // clang-format on
+  }
+  return total_score;
+}
+
+double TwoFingerGraspScoreWeights::getWeightTotal() const
+{
+  return GraspScoreWeights::getWeightTotal() + depth_score_weight_ + width_score_weight_;
+}
+
+double TwoFingerGraspScorer::scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open)
+{
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.graspWidth", "raw score = " << percent_open);
+  return pow(percent_open, 2);
+}
+
+double TwoFingerGraspScorer::scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp,
+                                                 const TwoFingerGraspDataPtr& grasp_data,
+                                                 const Eigen::Isometry3d& object_pose, double min_grasp_distance,
+                                                 double max_grasp_distance)
+{
+  double distance = (grasp_pose_tcp.translation() - object_pose.translation()).norm();
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance",
+                         "distance = " << distance << ", " << min_grasp_distance << ":" << max_grasp_distance);
+
+  double score = 1.0 - (distance - min_grasp_distance) / (max_grasp_distance - min_grasp_distance);
+
+  ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance", "raw score = " << score);
+  if (score < 0)
+    ROS_WARN_STREAM_NAMED("grasp_scorer.distance", "score < 0!");
+  return pow(score, 4);
+}
+
+}  // namespace moveit_grasps
diff --git a/src/panda_moveit_config/CMakeLists.txt b/src/panda_moveit_config/CMakeLists.txt
index b4e880c67048e6eadc6a48a994880593a9903906..43e77c5c2a0f8726547938192f29f5f10379eef9 100644
--- a/src/panda_moveit_config/CMakeLists.txt
+++ b/src/panda_moveit_config/CMakeLists.txt
@@ -3,6 +3,7 @@ project(panda_moveit_config)
 
 find_package(catkin REQUIRED)
 
+
 catkin_package()
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}