diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h
index 7a106d43ff..549b75a0de 100644
--- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h
+++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_torque_limit_parameterization.h
@@ -38,6 +38,8 @@
 
 #include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 
+#include <unordered_map>
+
 namespace trajectory_processing
 {
 class IterativeTorqueLimitParameterization
diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt
index 624960e1ad..9703ad463e 100644
--- a/moveit_ros/planning_interface/CMakeLists.txt
+++ b/moveit_ros/planning_interface/CMakeLists.txt
@@ -28,7 +28,7 @@ find_package(catkin REQUIRED COMPONENTS
   rosconsole
 )
 
-find_package(PythonInterp REQUIRED)
+find_package(PythonInterp ${CHEF_FORCE_PYTHON_VERSION} REQUIRED)
 find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED)
 
 find_package(Boost REQUIRED)
@@ -46,8 +46,11 @@ find_package(Boost REQUIRED COMPONENTS
   system
   thread
 )
+
+
 find_package(Eigen3 REQUIRED)
 find_package(eigenpy REQUIRED)
+# TODO: probably need different eigenpy
 
 set(THIS_PACKAGE_INCLUDE_DIRS
   py_bindings_tools/include
@@ -58,13 +61,19 @@ set(THIS_PACKAGE_INCLUDE_DIRS
 )
 
 catkin_python_setup()
+# Set boost_python_component if needed
 
+if("${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}" EQUAL "37")
+  set(Boost_PYTHON_LIBRARY "/usr/local/lib/libboost_python3.so")
+  set(Boost_LIBRARIES "${Boost_PYTHON_LIBRARY}")
+endif()
+link_directories("/usr/local/lib")
 catkin_package(
   LIBRARIES
     moveit_common_planning_interface_objects
     moveit_planning_scene_interface
     moveit_move_group_interface
-    moveit_py_bindings_tools
+    # moveit_py_bindings_tools_py${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}
     moveit_cpp
   INCLUDE_DIRS
     ${THIS_PACKAGE_INCLUDE_DIRS}
@@ -82,6 +91,8 @@ catkin_package(
     EIGEN3
 )
 
+list(REMOVE_ITEM catkin_LIBRARIES /usr/lib/x86_64-linux-gnu/libpython2.7.so)
+
 include_directories(${THIS_PACKAGE_INCLUDE_DIRS}
                     ${Boost_INCLUDE_DIRS}
                     ${catkin_INCLUDE_DIRS})
diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt
index 1f82d7e4dc..88f0a362c9 100644
--- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt
+++ b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt
@@ -1,8 +1,10 @@
 set(MOVEIT_LIB_NAME moveit_py_bindings_tools)
 
 add_library(${MOVEIT_LIB_NAME} src/roscpp_initializer.cpp)
+
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES})
+set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES OUTPUT_NAME moveit_py_bindings_tools_py${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}.so )
 
 install(TARGETS ${MOVEIT_LIB_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}