diff --git a/trajopt/CMakeLists.txt b/trajopt/CMakeLists.txt
index 92ac8538..f9699807 100644
--- a/trajopt/CMakeLists.txt
+++ b/trajopt/CMakeLists.txt
@@ -69,19 +69,7 @@ target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TRAJOPT_CXX_VERSION})
 target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                   "$<INSTALL_INTERFACE:include>")
 
-# Install test data directory for use in other packages
-add_library(${PROJECT_NAME}_test_data INTERFACE)
-target_link_libraries(${PROJECT_NAME}_test_data INTERFACE)
-target_compile_definitions(
-  ${PROJECT_NAME}_test_data INTERFACE ${TRAJOPT_COMPILE_DEFINITIONS}
-                                      TRAJOPT_DIR="${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}")
-target_cxx_version(${PROJECT_NAME}_test_data INTERFACE VERSION ${TRAJOPT_CXX_VERSION})
-
-configure_package(NAMESPACE trajopt TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_test_data)
-
-foreach(dir test/data)
-  install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir})
-endforeach()
+configure_package(NAMESPACE trajopt TARGETS ${PROJECT_NAME})
 
 # Mark cpp header files for installation
 install(
diff --git a/trajopt/test/CMakeLists.txt b/trajopt/test/CMakeLists.txt
index dda0d7a2..05483513 100644
--- a/trajopt/test/CMakeLists.txt
+++ b/trajopt/test/CMakeLists.txt
@@ -41,7 +41,7 @@ include(GoogleTest)
 macro(add_gtest test_name test_file)
   add_executable(${test_name} ${test_file})
   target_compile_options(${test_name} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
-  target_compile_definitions(${test_name} PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS} TRAJOPT_DIR="${CMAKE_SOURCE_DIR}")
+  target_compile_definitions(${test_name} PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS})
   target_cxx_version(${test_name} PRIVATE VERSION ${TRAJOPT_CXX_VERSION})
   target_clang_tidy(${test_name} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
   target_link_libraries(
@@ -70,29 +70,20 @@ add_gtest(${PROJECT_NAME}_cast_cost_octomap_unit cast_cost_octomap_unit.cpp)
 add_gtest(${PROJECT_NAME}_simple_collision_unit simple_collision_unit.cpp)
 add_gtest(${PROJECT_NAME}_cart_position_optimization_unit cart_position_optimization_unit.cpp)
 
-## This is for comparison and should be moved to trajopt
-#add_executable(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
-#               cart_position_optimization_trajopt_sco_unit.cpp)
-#target_compile_options(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
-#                       PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
-#target_compile_definitions(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
-#                           PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS} TRAJOPT_IFOPT_DIR="${CMAKE_SOURCE_DIR}")
-#target_cxx_version(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE VERSION ${TRAJOPT_CXX_VERSION})
-#target_clang_tidy(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
-#target_link_libraries(
-#  ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
-#  ${PROJECT_NAME}
-#  console_bridge
-#  ifopt::ifopt_ipopt
-#  trajopt::trajopt_ifopt
-#  trajopt::trajopt_test_data
-#  trajopt::trajopt
-#  GTest::GTest
-#  GTest::Main)
-#target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
-#                           PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>" ${GTEST_INCLUDE_DIRS})
-#target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit SYSTEM
-#                           PRIVATE ${PCL_INCLUDE_DIRS})
-#add_gtest_discover_tests(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit)
-#add_dependencies(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ${PROJECT_NAME})
-#add_dependencies(run_tests ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit)
+# This is for comparison and should be moved to trajopt
+# add_executable(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit
+# cart_position_optimization_trajopt_sco_unit.cpp)
+# target_compile_options(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE
+# ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
+# target_compile_definitions(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE
+# ${TRAJOPT_COMPILE_DEFINITIONS} TRAJOPT_IFOPT_DIR="${CMAKE_SOURCE_DIR}")
+# target_cxx_version(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE VERSION ${TRAJOPT_CXX_VERSION})
+# target_clang_tidy(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
+# target_link_libraries( ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ${PROJECT_NAME} console_bridge
+# ifopt::ifopt_ipopt trajopt::trajopt_ifopt trajopt::trajopt_test_data trajopt::trajopt GTest::GTest GTest::Main)
+# target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit PRIVATE
+# "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>" ${GTEST_INCLUDE_DIRS})
+# target_include_directories(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit SYSTEM PRIVATE
+# ${PCL_INCLUDE_DIRS}) add_gtest_discover_tests(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit)
+# add_dependencies(${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit ${PROJECT_NAME})
+# add_dependencies(run_tests ${PROJECT_NAME}_cart_position_optimization_trajopt_sco_unit)
diff --git a/trajopt/test/benchmarks/CMakeLists.txt b/trajopt/test/benchmarks/CMakeLists.txt
index ee372f6c..cdd50d30 100644
--- a/trajopt/test/benchmarks/CMakeLists.txt
+++ b/trajopt/test/benchmarks/CMakeLists.txt
@@ -4,7 +4,7 @@ find_package(osqp REQUIRED)
 macro(add_benchmark benchmark_name benchmark_file)
   add_executable(${benchmark_name} ${benchmark_file})
   target_compile_options(${benchmark_name} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
-  target_compile_definitions(${benchmark_name} PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS} TRAJOPT_DIR="${CMAKE_SOURCE_DIR}")
+  target_compile_definitions(${benchmark_name} PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS})
   target_cxx_version(${benchmark_name} PRIVATE VERSION ${TRAJOPT_CXX_VERSION})
   target_clang_tidy(${benchmark_name} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
   target_link_libraries(
diff --git a/trajopt/test/benchmarks/solve_benchmarks.cpp b/trajopt/test/benchmarks/solve_benchmarks.cpp
index 4562448d..2232ebf5 100644
--- a/trajopt/test/benchmarks/solve_benchmarks.cpp
+++ b/trajopt/test/benchmarks/solve_benchmarks.cpp
@@ -19,8 +19,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
 
-#include "../trajopt_test_utils.hpp"
-
 using namespace trajopt;
 using namespace std;
 using namespace trajopt_common;
@@ -98,11 +96,11 @@ int main(int argc, char** argv)
   // Simple Collision Solve
   //////////////////////////////////////
   {
-    Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/simple_collision_test.json");
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
+    Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/simple_collision_test.json");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     auto env = std::make_shared<Environment>();
     env->init(urdf_file, srdf_file, locator);
 
@@ -134,11 +132,11 @@ int main(int argc, char** argv)
   // Planning Solve
   //////////////////////////////////////
   {
-    Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/arm_around_table.json");
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     auto env = std::make_shared<Environment>();
     env->init(urdf_file, srdf_file, locator);
 
diff --git a/trajopt/test/cart_position_optimization_unit.cpp b/trajopt/test/cart_position_optimization_unit.cpp
index 7f602921..0ebebfb6 100644
--- a/trajopt/test/cart_position_optimization_unit.cpp
+++ b/trajopt/test/cart_position_optimization_unit.cpp
@@ -41,7 +41,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/problem_description.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/logging.hpp>
-#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace tesseract_environment;
@@ -66,9 +65,9 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco)
   }
 
   // 1)  Load Robot
-  tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-  tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
-  ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+  tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+  tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
+  ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
   auto env = std::make_shared<Environment>();
   env->init(urdf_file, srdf_file, locator);
 
diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp
index 0e0ddfef..17b674c4 100644
--- a/trajopt/test/cast_cost_attached_unit.cpp
+++ b/trajopt/test/cast_cost_attached_unit.cpp
@@ -15,11 +15,11 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include "trajopt_test_utils.hpp"
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -42,10 +42,10 @@ class CastAttachedTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
@@ -105,7 +105,7 @@ void runLinkWithGeomTest(const Environment::Ptr& env, const Visualization::Ptr&
 
   env->applyCommand(std::make_shared<ChangeLinkCollisionEnabledCommand>("box_attached", true));
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["boxbot_x_joint"] = -1.9;
@@ -165,7 +165,7 @@ void runLinkWithoutGeomTest(const Environment::Ptr& env, const Visualization::Pt
 
   env->applyCommand(std::make_shared<ChangeLinkCollisionEnabledCommand>("box_attached2", true));
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["boxbot_x_joint"] = -1.9;
diff --git a/trajopt/test/cast_cost_octomap_unit.cpp b/trajopt/test/cast_cost_octomap_unit.cpp
index 5436ea94..36a8ac1f 100644
--- a/trajopt/test/cast_cost_octomap_unit.cpp
+++ b/trajopt/test/cast_cost_octomap_unit.cpp
@@ -17,11 +17,11 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include "trajopt_test_utils.hpp"
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -44,10 +44,10 @@ class CastOctomapTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot_world.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
@@ -98,7 +98,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo
 {
   CONSOLE_BRIDGE_logDebug("CastOctomapTest, boxes");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["boxbot_x_joint"] = -1.9;
diff --git a/trajopt/test/cast_cost_unit.cpp b/trajopt/test/cast_cost_unit.cpp
index 0fc473b9..43c2fbb3 100644
--- a/trajopt/test/cast_cost_unit.cpp
+++ b/trajopt/test/cast_cost_unit.cpp
@@ -12,11 +12,11 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -39,10 +39,10 @@ class CastTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
@@ -56,7 +56,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo
 {
   CONSOLE_BRIDGE_logDebug("CastTest, boxes");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["boxbot_x_joint"] = -1.9;
diff --git a/trajopt/test/cast_cost_world_unit.cpp b/trajopt/test/cast_cost_world_unit.cpp
index c495acbc..1699eb93 100644
--- a/trajopt/test/cast_cost_world_unit.cpp
+++ b/trajopt/test/cast_cost_world_unit.cpp
@@ -13,11 +13,11 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include "trajopt_test_utils.hpp"
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -40,10 +40,10 @@ class CastWorldTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot_world.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
@@ -81,7 +81,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo
 {
   CONSOLE_BRIDGE_logDebug("CastWorldTest, boxes");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/box_cast_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/box_cast_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["boxbot_x_joint"] = -1.9;
diff --git a/trajopt/test/interface_unit.cpp b/trajopt/test/interface_unit.cpp
index 3aee36c7..5c8909f0 100644
--- a/trajopt/test/interface_unit.cpp
+++ b/trajopt/test/interface_unit.cpp
@@ -12,7 +12,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/logging.hpp>
 
 using namespace trajopt;
@@ -33,10 +32,10 @@ class InterfaceTest : public testing::TestWithParam<const char*>
   Visualization::Ptr plotter_;                             /**< Trajopt Plotter */
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
@@ -161,7 +160,7 @@ TEST_F(InterfaceTest, initial_trajectory_json_interface)
   // These must match the json file or the test will fail!!
   const int steps = 10;
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/arm_around_table.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["torso_lift_joint"] = 0;
@@ -196,7 +195,7 @@ TEST_F(InterfaceTest, initial_trajectory_time_json_interface)
   const int steps = 10;
   const double dt = 0.12341234;
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/arm_around_table.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["torso_lift_joint"] = 0;
diff --git a/trajopt/test/joint_costs_unit.cpp b/trajopt/test/joint_costs_unit.cpp
index ca3c0849..2f76b4b0 100644
--- a/trajopt/test/joint_costs_unit.cpp
+++ b/trajopt/test/joint_costs_unit.cpp
@@ -11,7 +11,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/clock.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
@@ -38,10 +37,10 @@ class CostsTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
diff --git a/trajopt/test/kinematic_costs_unit.cpp b/trajopt/test/kinematic_costs_unit.cpp
index 13792a19..3ea6a997 100644
--- a/trajopt/test/kinematic_costs_unit.cpp
+++ b/trajopt/test/kinematic_costs_unit.cpp
@@ -11,7 +11,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
@@ -37,10 +36,10 @@ class KinematicCostsTest : public testing::Test
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp
index 7c23d6f7..28275957 100644
--- a/trajopt/test/numerical_ik_unit.cpp
+++ b/trajopt/test/numerical_ik_unit.cpp
@@ -12,12 +12,12 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/clock.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -36,10 +36,10 @@ class NumericalIKTest : public testing::TestWithParam<const char*>
   Visualization::Ptr plotter_;                             /**< Trajopt Plotter */
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
@@ -57,7 +57,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& /*plotter*/,
 {
   CONSOLE_BRIDGE_logDebug("NumericalIKTest, numerical_ik1");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/numerical_ik1.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/numerical_ik1.json");
 
   //  plotter_->plotScene();
 
diff --git a/trajopt/test/planning_unit.cpp b/trajopt/test/planning_unit.cpp
index 6d31c29d..0022a6a5 100644
--- a/trajopt/test/planning_unit.cpp
+++ b/trajopt/test/planning_unit.cpp
@@ -12,12 +12,12 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt/plot_callback.hpp>
 #include <trajopt/problem_description.hpp>
 #include <trajopt_sco/optimizers.hpp>
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/clock.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
+#include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
 using namespace std;
@@ -38,10 +38,10 @@ class PlanningTest : public testing::TestWithParam<const char*>
   Visualization::Ptr plotter_;                             /**< Trajopt Plotter */
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
@@ -59,7 +59,7 @@ void runTest(const Environment::Ptr& env, bool use_multi_threaded)
 {
   CONSOLE_BRIDGE_logDebug("PlanningTest, arm_around_table");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/arm_around_table.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/arm_around_table.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["torso_lift_joint"] = 0;
diff --git a/trajopt/test/simple_collision_unit.cpp b/trajopt/test/simple_collision_unit.cpp
index 41feb228..1a8d497d 100644
--- a/trajopt/test/simple_collision_unit.cpp
+++ b/trajopt/test/simple_collision_unit.cpp
@@ -16,7 +16,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
 #include <trajopt_common/stl_to_string.hpp>
-
 #include "trajopt_test_utils.hpp"
 
 using namespace trajopt;
@@ -40,10 +39,10 @@ class SimpleCollisionTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env_->init(urdf_file, srdf_file, locator));
 
     // gLogLevel = trajopt_common::LevelDebug;
@@ -57,7 +56,7 @@ void runTest(const Environment::Ptr& env, const Visualization::Ptr& plotter, boo
 {
   CONSOLE_BRIDGE_logDebug("SimpleCollisionTest, spheres");
 
-  Json::Value root = readJsonFile(std::string(TRAJOPT_DIR) + "/test/data/config/simple_collision_test.json");
+  Json::Value root = readJsonFile(std::string(TRAJOPT_DATA_DIR) + "/config/simple_collision_test.json");
 
   std::unordered_map<std::string, double> ipos;
   ipos["spherebot_x_joint"] = -0.75;
diff --git a/trajopt/test/trajopt_test_utils.hpp b/trajopt/test/trajopt_test_utils.hpp
index 3da6e76b..d4d47f59 100644
--- a/trajopt/test/trajopt_test_utils.hpp
+++ b/trajopt/test/trajopt_test_utils.hpp
@@ -4,7 +4,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <json/json.h>
 #include <stdexcept>
 #include <Eigen/Core>
-#include <tesseract_common/resource_locator.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt/typedefs.hpp>
@@ -19,37 +18,3 @@ inline Json::Value readJsonFile(const std::string& fname)
     throw std::runtime_error("failed to parse " + fname);
   return root;
 }
-
-class TrajOptSupportResourceLocator : public tesseract_common::ResourceLocator
-{
-public:
-  using Ptr = std::shared_ptr<TrajOptSupportResourceLocator>;
-  using ConstPtr = std::shared_ptr<const TrajOptSupportResourceLocator>;
-
-  std::shared_ptr<tesseract_common::Resource> locateResource(const std::string& url) const override
-  {
-    std::string mod_url = url;
-    if (url.find("package://trajopt") == 0)
-    {
-      mod_url.erase(0, strlen("package://trajopt"));
-      size_t pos = mod_url.find('/');
-      if (pos == std::string::npos)
-        return nullptr;
-
-      std::string package = mod_url.substr(0, pos);
-      mod_url.erase(0, pos);
-      std::string package_path = std::string(TRAJOPT_DIR);
-
-      if (package_path.empty())
-        return nullptr;
-
-      mod_url = package_path + mod_url;
-    }
-
-    if (!tesseract_common::fs::path(mod_url).is_complete())
-      return nullptr;
-
-    return std::make_shared<tesseract_common::SimpleLocatedResource>(
-        url, mod_url, std::make_shared<TrajOptSupportResourceLocator>(*this));
-  }
-};
diff --git a/trajopt_common/CMakeLists.txt b/trajopt_common/CMakeLists.txt
index 608dfd24..4c058afd 100644
--- a/trajopt_common/CMakeLists.txt
+++ b/trajopt_common/CMakeLists.txt
@@ -30,7 +30,8 @@ add_library(${PROJECT_NAME} ${UTILS_SOURCE_FILES})
 target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::program_options tesseract::tesseract_common)
 target_compile_options(${PROJECT_NAME} PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_OPTIONS_PUBLIC})
-target_compile_definitions(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS})
+target_compile_definitions(${PROJECT_NAME} PUBLIC ${TRAJOPT_COMPILE_DEFINITIONS}
+                                                  TRAJOPT_DATA_DIR="${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/data")
 target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${TRAJOPT_CXX_VERSION})
 target_clang_tidy(${PROJECT_NAME} ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY})
 target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -49,6 +50,10 @@ install(
 
 install(FILES "${CMAKE_CURRENT_LIST_DIR}/cmake/trajopt_macros.cmake" DESTINATION lib/cmake/${PROJECT_NAME})
 
+foreach(dir data)
+  install(DIRECTORY ${dir}/ DESTINATION share/${PROJECT_NAME}/${dir})
+endforeach()
+
 if(TRAJOPT_PACKAGE)
   cpack(
     VERSION
diff --git a/trajopt/test/data/arm_around_table.urdf b/trajopt_common/data/arm_around_table.urdf
similarity index 93%
rename from trajopt/test/data/arm_around_table.urdf
rename to trajopt_common/data/arm_around_table.urdf
index 9cb566cb..77e6f6c1 100644
--- a/trajopt/test/data/arm_around_table.urdf
+++ b/trajopt_common/data/arm_around_table.urdf
@@ -56,23 +56,23 @@
     <color rgba="0.6 0.6 0.6 1.0"/>
   </material>
   <material name="Caster">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_caster_texture.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_caster_texture.png"/>
   </material>
   <material name="Wheel_l">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png"/>
   </material>
   <material name="Wheel_r">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_right.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_right.png"/>
   </material>
   <material name="RollLinks">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png"/>
   </material>
 
   <link name="table_link">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://trajopt/test/data/meshes/Table.stl"/>
+        <mesh filename="package://trajopt_common/data/meshes/Table.stl"/>
       </geometry>
       <material name="Cyan">
         <color rgba="0 1.0 1.0 1.0"/>
@@ -82,7 +82,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/meshes/Table.stl"/>
+        <mesh filename="package://trajopt_common/data/meshes/Table.stl"/>
       </geometry>
     </collision>
   </link>
@@ -103,14 +103,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/base.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/base.dae"/>
       </geometry>
       <material name="White"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/base_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/base_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -213,14 +213,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -248,7 +248,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -292,7 +292,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -340,14 +340,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -375,7 +375,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -419,7 +419,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -467,14 +467,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -502,7 +502,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -546,7 +546,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -594,14 +594,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -629,7 +629,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -673,7 +673,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -752,14 +752,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/torso_v0/torso_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/torso_v0/torso_lift.dae"/>
       </geometry>
       <material name="Grey2"/>
     </visual>
     <collision name="torso_lift_collision">
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/torso_v0/torso_lift_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/torso_v0/torso_lift_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -896,13 +896,13 @@
     <visual>
       <origin rpy="0 0 0 " xyz="0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_pan_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_pan_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -932,14 +932,14 @@
     <visual>
       <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_tilt.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_tilt.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_tilt_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_tilt_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1413,14 +1413,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1498,14 +1498,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1531,14 +1531,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1564,14 +1564,14 @@
       <!-- TODO: This component doesn't actually have a mesh -->
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1635,14 +1635,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1674,7 +1674,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
@@ -1682,7 +1682,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1720,14 +1720,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1764,14 +1764,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1796,14 +1796,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1829,14 +1829,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1880,14 +1880,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1991,14 +1991,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2026,14 +2026,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2061,14 +2061,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2096,14 +2096,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2415,14 +2415,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2448,14 +2448,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2481,14 +2481,14 @@
       <!-- TODO: This component doesn't actually have a mesh -->
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2552,14 +2552,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2591,7 +2591,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
@@ -2599,7 +2599,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2637,14 +2637,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2681,14 +2681,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2713,14 +2713,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2746,14 +2746,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2797,14 +2797,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2908,14 +2908,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2943,14 +2943,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2978,14 +2978,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -3013,14 +3013,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
diff --git a/trajopt/test/data/box_1m_macro.xacro b/trajopt_common/data/box_1m_macro.xacro
similarity index 100%
rename from trajopt/test/data/box_1m_macro.xacro
rename to trajopt_common/data/box_1m_macro.xacro
diff --git a/trajopt/test/data/boxbot.srdf b/trajopt_common/data/boxbot.srdf
similarity index 73%
rename from trajopt/test/data/boxbot.srdf
rename to trajopt_common/data/boxbot.srdf
index fd9d8430..858342a2 100644
--- a/trajopt/test/data/boxbot.srdf
+++ b/trajopt_common/data/boxbot.srdf
@@ -5,7 +5,7 @@
     </group>
 
     <!--Contact Managers plugins -->
-    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+    <contact_managers_plugin_config filename="package://trajopt_common/data/contact_manager_plugins.yaml"/>
 
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="base_link" />
 </robot>
diff --git a/trajopt/test/data/boxbot.urdf b/trajopt_common/data/boxbot.urdf
similarity index 100%
rename from trajopt/test/data/boxbot.urdf
rename to trajopt_common/data/boxbot.urdf
diff --git a/trajopt/test/data/boxbot.xacro b/trajopt_common/data/boxbot.xacro
similarity index 100%
rename from trajopt/test/data/boxbot.xacro
rename to trajopt_common/data/boxbot.xacro
diff --git a/trajopt/test/data/boxbot_macro.xacro b/trajopt_common/data/boxbot_macro.xacro
similarity index 100%
rename from trajopt/test/data/boxbot_macro.xacro
rename to trajopt_common/data/boxbot_macro.xacro
diff --git a/trajopt/test/data/boxbot_world.urdf b/trajopt_common/data/boxbot_world.urdf
similarity index 100%
rename from trajopt/test/data/boxbot_world.urdf
rename to trajopt_common/data/boxbot_world.urdf
diff --git a/trajopt/test/data/boxbot_world.xacro b/trajopt_common/data/boxbot_world.xacro
similarity index 100%
rename from trajopt/test/data/boxbot_world.xacro
rename to trajopt_common/data/boxbot_world.xacro
diff --git a/trajopt/test/data/config/arm_around_table.json b/trajopt_common/data/config/arm_around_table.json
similarity index 100%
rename from trajopt/test/data/config/arm_around_table.json
rename to trajopt_common/data/config/arm_around_table.json
diff --git a/trajopt/test/data/config/arm_around_table_continuous.json b/trajopt_common/data/config/arm_around_table_continuous.json
similarity index 100%
rename from trajopt/test/data/config/arm_around_table_continuous.json
rename to trajopt_common/data/config/arm_around_table_continuous.json
diff --git a/trajopt/test/data/config/arm_around_table_time.json b/trajopt_common/data/config/arm_around_table_time.json
similarity index 100%
rename from trajopt/test/data/config/arm_around_table_time.json
rename to trajopt_common/data/config/arm_around_table_time.json
diff --git a/trajopt/test/data/config/box_cast_test.json b/trajopt_common/data/config/box_cast_test.json
similarity index 100%
rename from trajopt/test/data/config/box_cast_test.json
rename to trajopt_common/data/config/box_cast_test.json
diff --git a/trajopt/test/data/config/numerical_ik1.json b/trajopt_common/data/config/numerical_ik1.json
similarity index 100%
rename from trajopt/test/data/config/numerical_ik1.json
rename to trajopt_common/data/config/numerical_ik1.json
diff --git a/trajopt/test/data/config/simple_collision_test.json b/trajopt_common/data/config/simple_collision_test.json
similarity index 100%
rename from trajopt/test/data/config/simple_collision_test.json
rename to trajopt_common/data/config/simple_collision_test.json
diff --git a/trajopt/test/data/contact_manager_plugins.yaml b/trajopt_common/data/contact_manager_plugins.yaml
similarity index 100%
rename from trajopt/test/data/contact_manager_plugins.yaml
rename to trajopt_common/data/contact_manager_plugins.yaml
diff --git a/trajopt/test/data/meshes/Table.stl b/trajopt_common/data/meshes/Table.stl
similarity index 100%
rename from trajopt/test/data/meshes/Table.stl
rename to trajopt_common/data/meshes/Table.stl
diff --git a/trajopt/test/data/pr2.srdf b/trajopt_common/data/pr2.srdf
similarity index 99%
rename from trajopt/test/data/pr2.srdf
rename to trajopt_common/data/pr2.srdf
index 1ecabad2..e07e2c0f 100644
--- a/trajopt/test/data/pr2.srdf
+++ b/trajopt_common/data/pr2.srdf
@@ -17,10 +17,10 @@
     </group>
 
     <!--Groups kinematic plugins -->
-    <kinematics_plugin_config filename="package://trajopt/test/data/pr2_plugins.yaml"/>
+    <kinematics_plugin_config filename="package://trajopt_common/data/pr2_plugins.yaml"/>
 
     <!--Contact Managers plugins -->
-    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+    <contact_managers_plugin_config filename="package://trajopt_common/data/contact_manager_plugins.yaml"/>
 
     <group name="right_gripper">
         <link name="r_gripper_palm_link" />
diff --git a/trajopt/test/data/pr2.urdf b/trajopt_common/data/pr2.urdf
similarity index 94%
rename from trajopt/test/data/pr2.urdf
rename to trajopt_common/data/pr2.urdf
index d4b64eaa..0b22cc67 100644
--- a/trajopt/test/data/pr2.urdf
+++ b/trajopt_common/data/pr2.urdf
@@ -56,16 +56,16 @@
     <color rgba="0.6 0.6 0.6 1.0"/>
   </material>
   <material name="Caster">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_caster_texture.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_caster_texture.png"/>
   </material>
   <material name="Wheel_l">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png"/>
   </material>
   <material name="Wheel_r">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_right.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_right.png"/>
   </material>
   <material name="RollLinks">
-    <texture filename="package://trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png"/>
   </material>
   <!-- Now we can start using the macros included above to define the actual PR2 -->
   <link name="base_link">
@@ -77,14 +77,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/base.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/base.dae"/>
       </geometry>
       <material name="White"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/base_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/base_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -207,14 +207,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -242,7 +242,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -286,7 +286,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -334,14 +334,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -369,7 +369,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -413,7 +413,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -461,14 +461,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -496,7 +496,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -540,7 +540,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -588,14 +588,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster.stl"/>
       </geometry>
       <material name="Caster"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/caster_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/caster_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -623,7 +623,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -667,7 +667,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/base_v0/wheel.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -746,14 +746,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/torso_v0/torso_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/torso_v0/torso_lift.dae"/>
       </geometry>
       <material name="Grey2"/>
     </visual>
     <collision name="torso_lift_collision">
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/torso_v0/torso_lift_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/torso_v0/torso_lift_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -890,13 +890,13 @@
     <visual>
       <origin rpy="0 0 0 " xyz="0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_pan_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_pan_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -926,14 +926,14 @@
     <visual>
       <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_tilt.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_tilt.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/head_v0/head_tilt_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/head_v0/head_tilt_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1439,14 +1439,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1524,14 +1524,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1557,14 +1557,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1590,14 +1590,14 @@
       <!-- TODO: This component doesn't actually have a mesh -->
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1661,14 +1661,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1700,7 +1700,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
@@ -1708,7 +1708,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1746,14 +1746,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1790,14 +1790,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1822,14 +1822,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1855,14 +1855,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -1906,14 +1906,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2034,14 +2034,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2069,14 +2069,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2104,14 +2104,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2139,14 +2139,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2458,14 +2458,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2491,14 +2491,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2524,14 +2524,14 @@
       <!-- TODO: This component doesn't actually have a mesh -->
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2595,14 +2595,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2634,7 +2634,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
@@ -2642,7 +2642,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2680,14 +2680,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2724,14 +2724,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2756,14 +2756,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2789,14 +2789,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl"/>
       </geometry>
       <material name="RollLinks"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2840,14 +2840,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl"/>
       </geometry>
     </collision>
   </link>
@@ -2968,14 +2968,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -3003,14 +3003,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -3038,14 +3038,14 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
@@ -3073,14 +3073,14 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
     <collision>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl"/>
       </geometry>
     </collision>
   </link>
diff --git a/trajopt/test/data/pr2/materials/textures/pr2_caster_texture.png b/trajopt_common/data/pr2/materials/textures/pr2_caster_texture.png
similarity index 100%
rename from trajopt/test/data/pr2/materials/textures/pr2_caster_texture.png
rename to trajopt_common/data/pr2/materials/textures/pr2_caster_texture.png
diff --git a/trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png b/trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png
similarity index 100%
rename from trajopt/test/data/pr2/materials/textures/pr2_wheel_left.png
rename to trajopt_common/data/pr2/materials/textures/pr2_wheel_left.png
diff --git a/trajopt/test/data/pr2/materials/textures/pr2_wheel_right.png b/trajopt_common/data/pr2/materials/textures/pr2_wheel_right.png
similarity index 100%
rename from trajopt/test/data/pr2/materials/textures/pr2_wheel_right.png
rename to trajopt_common/data/pr2/materials/textures/pr2_wheel_right.png
diff --git a/trajopt/test/data/pr2/meshes/base_v0/base.dae b/trajopt_common/data/pr2/meshes/base_v0/base.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/base.dae
rename to trajopt_common/data/pr2/meshes/base_v0/base.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/base.stl b/trajopt_common/data/pr2/meshes/base_v0/base.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/base.stl
rename to trajopt_common/data/pr2/meshes/base_v0/base.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/base_L.stl b/trajopt_common/data/pr2/meshes/base_v0/base_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/base_L.stl
rename to trajopt_common/data/pr2/meshes/base_v0/base_L.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/base_color.tif b/trajopt_common/data/pr2/meshes/base_v0/base_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/base_color.tif
rename to trajopt_common/data/pr2/meshes/base_v0/base_color.tif
diff --git a/trajopt/test/data/pr2/meshes/base_v0/base_normals.tif b/trajopt_common/data/pr2/meshes/base_v0/base_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/base_normals.tif
rename to trajopt_common/data/pr2/meshes/base_v0/base_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/base_v0/caster.stl b/trajopt_common/data/pr2/meshes/base_v0/caster.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/caster.stl
rename to trajopt_common/data/pr2/meshes/base_v0/caster.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/caster_L.stl b/trajopt_common/data/pr2/meshes/base_v0/caster_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/caster_L.stl
rename to trajopt_common/data/pr2/meshes/base_v0/caster_L.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/base.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/base.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/base.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/base_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/base_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/caster.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/caster.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/caster.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/caster_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/caster_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/pr2_wheel_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel.dae b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel.dae
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel.xml b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel.xml
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel.xml
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.iv b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.iv
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.obj b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.obj
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.stla b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.stla
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.stlb b/trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/convex/wheel_convex.stlb
rename to trajopt_common/data/pr2/meshes/base_v0/convex/wheel_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/base.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/base.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/base.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/base.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/base_L.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/base_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/base_L.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/base_L.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/caster.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/caster.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/caster.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/caster.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/caster_L.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/caster_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/caster_L.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/caster_L.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/pr2_wheel.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/pr2_wheel.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/pr2_wheel.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/pr2_wheel.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/iv/wheel.iv b/trajopt_common/data/pr2/meshes/base_v0/iv/wheel.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/iv/wheel.iv
rename to trajopt_common/data/pr2/meshes/base_v0/iv/wheel.iv
diff --git a/trajopt/test/data/pr2/meshes/base_v0/pr2_wheel.stl b/trajopt_common/data/pr2/meshes/base_v0/pr2_wheel.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/pr2_wheel.stl
rename to trajopt_common/data/pr2/meshes/base_v0/pr2_wheel.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel.dae b/trajopt_common/data/pr2/meshes/base_v0/wheel.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel.dae
rename to trajopt_common/data/pr2/meshes/base_v0/wheel.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel.stl b/trajopt_common/data/pr2/meshes/base_v0/wheel.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel.stl
rename to trajopt_common/data/pr2/meshes/base_v0/wheel.stl
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel_color.tif b/trajopt_common/data/pr2/meshes/base_v0/wheel_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel_color.tif
rename to trajopt_common/data/pr2/meshes/base_v0/wheel_color.tif
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel_h.dae b/trajopt_common/data/pr2/meshes/base_v0/wheel_h.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel_h.dae
rename to trajopt_common/data/pr2/meshes/base_v0/wheel_h.dae
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel_h_color.tif b/trajopt_common/data/pr2/meshes/base_v0/wheel_h_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel_h_color.tif
rename to trajopt_common/data/pr2/meshes/base_v0/wheel_h_color.tif
diff --git a/trajopt/test/data/pr2/meshes/base_v0/wheel_normals.tif b/trajopt_common/data/pr2/meshes/base_v0/wheel_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/base_v0/wheel_normals.tif
rename to trajopt_common/data/pr2/meshes/base_v0/wheel_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.dae b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.xml b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm.xml
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm.xml
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.iv b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.stla b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.stla
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.stlb b/trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/forearm_convex.stlb
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/forearm_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.dae b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.xml b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex.xml
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex.xml
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.iv b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stla b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stla
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stlb b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stlb
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_flex_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.dae b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.xml b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll.xml
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll.xml
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.dae b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.xml b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.xml
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L.xml
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.iv b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stla b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stla
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stlb b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.iv b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.obj b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.obj
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stla b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stla
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stlb b/trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stlb
rename to trajopt_common/data/pr2/meshes/forearm_v0/convex/wrist_roll_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae b/trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/forearm.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/forearm.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/forearm.jpg b/trajopt_common/data/pr2/meshes/forearm_v0/forearm.jpg
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/forearm.jpg
rename to trajopt_common/data/pr2/meshes/forearm_v0/forearm.jpg
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl b/trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/forearm.stl
rename to trajopt_common/data/pr2/meshes/forearm_v0/forearm.stl
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/forearm_color.tif b/trajopt_common/data/pr2/meshes/forearm_v0/forearm_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/forearm_color.tif
rename to trajopt_common/data/pr2/meshes/forearm_v0/forearm_color.tif
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/forearm_normals.tif b/trajopt_common/data/pr2/meshes/forearm_v0/forearm_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/forearm_normals.tif
rename to trajopt_common/data/pr2/meshes/forearm_v0/forearm_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/iv/forearm.iv b/trajopt_common/data/pr2/meshes/forearm_v0/iv/forearm.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/iv/forearm.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/iv/forearm.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_flex.iv b/trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_flex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_flex.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_flex.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_roll.iv b/trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_roll.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_roll.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_roll.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_roll_L.iv b/trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_roll_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/iv/wrist_roll_L.iv
rename to trajopt_common/data/pr2/meshes/forearm_v0/iv/wrist_roll_L.iv
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_color.tif b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_color.tif
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_color.tif
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.dae
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.dae
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_flex.stl
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_flex.stl
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_normals.tif b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_normals.tif
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll.stl
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll.stl
diff --git a/trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl b/trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/forearm_v0/wrist_roll_L.stl
rename to trajopt_common/data/pr2/meshes/forearm_v0/wrist_roll_L.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_l_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_l_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_pad2_r_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/finger_tip_r_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/gripper_palm_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_finger_tip_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/l_floating_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_l_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.dae b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.xml b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r.xml
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r.xml
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.iv b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.obj b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.obj
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stla b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stla
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stlb b/trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stlb
rename to trajopt_common/data/pr2/meshes/gripper_v0/convex/upper_finger_r_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_H_Color_100430.tif b/trajopt_common/data/pr2/meshes/gripper_v0/finger_H_Color_100430.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_H_Color_100430.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_H_Color_100430.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_H_UV_100430.dae b/trajopt_common/data/pr2/meshes/gripper_v0/finger_H_UV_100430.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_H_UV_100430.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_H_UV_100430.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_H_UV_100430.dae b/trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_H_UV_100430.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_H_UV_100430.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_H_UV_100430.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_l.stl b/trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_l.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_l.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_l.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_pad2_l.stl b/trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_pad2_l.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_pad2_l.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_pad2_l.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_pad2_r.stl b/trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_pad2_r.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_pad2_r.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_pad2_r.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_r.stl b/trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_r.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/finger_tip_r.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/finger_tip_r.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/fingertip_H_Color_100430.tif b/trajopt_common/data/pr2/meshes/gripper_v0/fingertip_H_Color_100430.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/fingertip_H_Color_100430.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/fingertip_H_Color_100430.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/float_H_Color_100430.tif b/trajopt_common/data/pr2/meshes/gripper_v0/float_H_Color_100430.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/float_H_Color_100430.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/float_H_Color_100430.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/float_H_UV_100430.dae b/trajopt_common/data/pr2/meshes/gripper_v0/float_H_UV_100430.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/float_H_UV_100430.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/float_H_UV_100430.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae b/trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl b/trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm_color.tif b/trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm_color.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm_color.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm_normals.tif b/trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/gripper_palm_normals.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/gripper_palm_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_l.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_l.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_l.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_l.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_l.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_l.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_l.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_l.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_r.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_r.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_r.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_pad2_r.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_r.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_r.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/finger_tip_r.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/finger_tip_r.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/gripper_palm.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/gripper_palm.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/gripper_palm.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/gripper_palm.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/l_finger.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/l_finger.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/l_finger.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/l_finger.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/l_finger_tip.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/l_finger_tip.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/l_finger_tip.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/l_finger_tip.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/l_floating.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/l_floating.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/l_floating.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/l_floating.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/upper_finger_l.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/upper_finger_l.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/upper_finger_l.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/upper_finger_l.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/iv/upper_finger_r.iv b/trajopt_common/data/pr2/meshes/gripper_v0/iv/upper_finger_r.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/iv/upper_finger_r.iv
rename to trajopt_common/data/pr2/meshes/gripper_v0/iv/upper_finger_r.iv
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_color.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_color.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_color.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_normals.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_normals.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip_color.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip_color.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip_color.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip_normals.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_finger_tip_normals.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_finger_tip_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_float.dae b/trajopt_common/data/pr2/meshes/gripper_v0/l_float.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_float.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_float.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_float_color.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_float_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_float_color.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_float_color.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_float_normals.tif b/trajopt_common/data/pr2/meshes/gripper_v0/l_float_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_float_normals.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_float_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/l_floating.stl b/trajopt_common/data/pr2/meshes/gripper_v0/l_floating.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/l_floating.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/l_floating.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/palm_H_Color_100430.tif b/trajopt_common/data/pr2/meshes/gripper_v0/palm_H_Color_100430.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/palm_H_Color_100430.tif
rename to trajopt_common/data/pr2/meshes/gripper_v0/palm_H_Color_100430.tif
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/palm_H_UV_100430.dae b/trajopt_common/data/pr2/meshes/gripper_v0/palm_H_UV_100430.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/palm_H_UV_100430.dae
rename to trajopt_common/data/pr2/meshes/gripper_v0/palm_H_UV_100430.dae
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/upper_finger_l.stl b/trajopt_common/data/pr2/meshes/gripper_v0/upper_finger_l.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/upper_finger_l.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/upper_finger_l.stl
diff --git a/trajopt/test/data/pr2/meshes/gripper_v0/upper_finger_r.stl b/trajopt_common/data/pr2/meshes/gripper_v0/upper_finger_r.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/gripper_v0/upper_finger_r.stl
rename to trajopt_common/data/pr2/meshes/gripper_v0/upper_finger_r.stl
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.dae b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.dae
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.xml b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan.xml
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan.xml
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.dae b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.dae
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.xml b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L.xml
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L.xml
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.iv b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.iv
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stla b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stla
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stlb b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.iv b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.iv
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.stla b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.stla
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.stlb b/trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_pan_convex.stlb
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_pan_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.dae b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.dae
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.xml b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt.xml
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt.xml
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.dae b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.dae
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.xml b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L.xml
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L.xml
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.iv b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.iv
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stla b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stla
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stlb b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.iv b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.iv
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.obj b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.obj
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.stla b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.stla
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.stlb b/trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/convex/head_tilt_convex.stlb
rename to trajopt_common/data/pr2/meshes/head_v0/convex/head_tilt_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_pan.dae b/trajopt_common/data/pr2/meshes/head_v0/head_pan.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_pan.dae
rename to trajopt_common/data/pr2/meshes/head_v0/head_pan.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_pan.stl b/trajopt_common/data/pr2/meshes/head_v0/head_pan.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_pan.stl
rename to trajopt_common/data/pr2/meshes/head_v0/head_pan.stl
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_pan_L.stl b/trajopt_common/data/pr2/meshes/head_v0/head_pan_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_pan_L.stl
rename to trajopt_common/data/pr2/meshes/head_v0/head_pan_L.stl
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_pan_color.tif b/trajopt_common/data/pr2/meshes/head_v0/head_pan_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_pan_color.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_pan_color.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_pan_normals.tif b/trajopt_common/data/pr2/meshes/head_v0/head_pan_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_pan_normals.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_pan_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt.dae b/trajopt_common/data/pr2/meshes/head_v0/head_tilt.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt.dae
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt.dae
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt.stl b/trajopt_common/data/pr2/meshes/head_v0/head_tilt.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt.stl
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt.stl
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_L.stl b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_L.stl
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_L.stl
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_color.tif b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_color.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_color.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_color_red.tif b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_color_red.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_color_red.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_color_red.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_color_yellow.tif b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_color_yellow.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_color_yellow.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_color_yellow.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_green.tif b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_green.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_green.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_green.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/head_tilt_normals.tif b/trajopt_common/data/pr2/meshes/head_v0/head_tilt_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/head_tilt_normals.tif
rename to trajopt_common/data/pr2/meshes/head_v0/head_tilt_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/head_v0/iv/head_pan.iv b/trajopt_common/data/pr2/meshes/head_v0/iv/head_pan.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/iv/head_pan.iv
rename to trajopt_common/data/pr2/meshes/head_v0/iv/head_pan.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/iv/head_pan_L.iv b/trajopt_common/data/pr2/meshes/head_v0/iv/head_pan_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/iv/head_pan_L.iv
rename to trajopt_common/data/pr2/meshes/head_v0/iv/head_pan_L.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/iv/head_tilt.iv b/trajopt_common/data/pr2/meshes/head_v0/iv/head_tilt.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/iv/head_tilt.iv
rename to trajopt_common/data/pr2/meshes/head_v0/iv/head_tilt.iv
diff --git a/trajopt/test/data/pr2/meshes/head_v0/iv/head_tilt_L.iv b/trajopt_common/data/pr2/meshes/head_v0/iv/head_tilt_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/head_v0/iv/head_tilt_L.iv
rename to trajopt_common/data/pr2/meshes/head_v0/iv/head_tilt_L.iv
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect2_v0/kinect2_assembly.STL b/trajopt_common/data/pr2/meshes/sensors/kinect2_v0/kinect2_assembly.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect2_v0/kinect2_assembly.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect2_v0/kinect2_assembly.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back_no_sensors--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back_no_sensors--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back_no_sensors--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back_no_sensors--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd_no_sensors--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd_no_sensors--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd_no_sensors--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_fwd_no_sensors--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/68-04546_Kinect_Sensor--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/68-04546_Kinect_Sensor--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/68-04546_Kinect_Sensor--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/68-04546_Kinect_Sensor--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/Prosilica_w_Lens--coarse.STL b/trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/Prosilica_w_Lens--coarse.STL
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_prosilica_v0/Prosilica_w_Lens--coarse.STL
rename to trajopt_common/data/pr2/meshes/sensors/kinect_prosilica_v0/Prosilica_w_Lens--coarse.STL
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect.dae b/trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect.dae
rename to trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect.dae
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect.tga b/trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect.tga
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect.tga
rename to trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect.tga
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect_color.tga b/trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect_color.tga
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect_color.tga
rename to trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect_color.tga
diff --git a/trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect_mount.stl b/trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect_mount.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/sensors/kinect_v0/kinect_mount.stl
rename to trajopt_common/data/pr2/meshes/sensors/kinect_v0/kinect_mount.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.xml b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.xml
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift.xml
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stla b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stla
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.xml b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.xml
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan.xml
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stla b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stla
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stlb b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stlb
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_pan_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.xml b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.xml
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw.xml
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stla b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stla
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.xml b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.xml
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll.xml
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.xml b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.xml
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L.xml
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stla b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stla
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stlb b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.obj b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.obj
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stla b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stla
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stlb b/trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stlb
rename to trajopt_common/data/pr2/meshes/shoulder_v0/convex/upper_arm_roll_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_lift.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_lift.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_lift.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_lift.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_pan.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_pan.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_pan.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_pan.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_yaw.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_yaw.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/iv/shoulder_yaw.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/iv/shoulder_yaw.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll_L.iv b/trajopt_common/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll_L.iv
rename to trajopt_common/data/pr2/meshes/shoulder_v0/iv/upper_arm_roll_L.iv
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift.stl
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift_color.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift_color.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift_color.tif
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift_normals.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_lift_normals.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_lift_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan.stl
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan_color.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan_color.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan_color.tif
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan_normals.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_pan_normals.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_pan_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_yaw.stl b/trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_yaw.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/shoulder_yaw.stl
rename to trajopt_common/data/pr2/meshes/shoulder_v0/shoulder_yaw.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.dae b/trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.dae
rename to trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.dae
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl b/trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl
rename to trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl b/trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl
rename to trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_L.stl
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_color.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_color.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_color.tif
diff --git a/trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_normals.tif b/trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/shoulder_v0/upper_arm_roll_normals.tif
rename to trajopt_common/data/pr2/meshes/shoulder_v0/upper_arm_roll_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.dae b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.dae
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.dae
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.xml b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.xml
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt.xml
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stla b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stla
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stlb b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stlb
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/hok_tilt_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.dae b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.dae
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.dae
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.xml b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.xml
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo.xml
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.dae b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.dae
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.dae
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.xml b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.xml
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L.xml
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stla b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stla
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stlb b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.obj b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.obj
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stla b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stla
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stlb b/trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stlb
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/convex/tilting_hokuyo_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/hok_tilt.stl b/trajopt_common/data/pr2/meshes/tilting_laser_v0/hok_tilt.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/hok_tilt.stl
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/hok_tilt.stl
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/hok_tilt.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/hok_tilt.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/hok_tilt.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/hok_tilt.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo_L.iv b/trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo_L.iv
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/iv/tilting_hokuyo_L.iv
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae b/trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.stl b/trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.stl
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo.stl
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl b/trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_color.tif b/trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_color.tif
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_color.tif
diff --git a/trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_normals.tif b/trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_normals.tif
rename to trajopt_common/data/pr2/meshes/tilting_laser_v0/tilting_hokuyo_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso.dae b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso.dae
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso.dae
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso.xml b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso.xml
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso.xml
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.iv b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.stla b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.stla
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.stlb b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_convex.stlb
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.dae b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.dae
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.dae
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.xml b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift.xml
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift.xml
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.dae b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.dae
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.dae
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.xml b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L.xml
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L.xml
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.iv b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stla b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stla
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stlb b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.iv b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.obj b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.obj
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stla b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stla
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stlb b/trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stlb
rename to trajopt_common/data/pr2/meshes/torso_v0/convex/torso_lift_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/iv/torso.iv b/trajopt_common/data/pr2/meshes/torso_v0/iv/torso.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/iv/torso.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/iv/torso.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/iv/torso_lift.iv b/trajopt_common/data/pr2/meshes/torso_v0/iv/torso_lift.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/iv/torso_lift.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/iv/torso_lift.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/iv/torso_lift_L.iv b/trajopt_common/data/pr2/meshes/torso_v0/iv/torso_lift_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/iv/torso_lift_L.iv
rename to trajopt_common/data/pr2/meshes/torso_v0/iv/torso_lift_L.iv
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso.stl b/trajopt_common/data/pr2/meshes/torso_v0/torso.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso.stl
rename to trajopt_common/data/pr2/meshes/torso_v0/torso.stl
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso_lift.dae b/trajopt_common/data/pr2/meshes/torso_v0/torso_lift.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso_lift.dae
rename to trajopt_common/data/pr2/meshes/torso_v0/torso_lift.dae
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso_lift.stl b/trajopt_common/data/pr2/meshes/torso_v0/torso_lift.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso_lift.stl
rename to trajopt_common/data/pr2/meshes/torso_v0/torso_lift.stl
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso_lift_L.stl b/trajopt_common/data/pr2/meshes/torso_v0/torso_lift_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso_lift_L.stl
rename to trajopt_common/data/pr2/meshes/torso_v0/torso_lift_L.stl
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso_lift_color.tif b/trajopt_common/data/pr2/meshes/torso_v0/torso_lift_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso_lift_color.tif
rename to trajopt_common/data/pr2/meshes/torso_v0/torso_lift_color.tif
diff --git a/trajopt/test/data/pr2/meshes/torso_v0/torso_lift_normals.tif b/trajopt_common/data/pr2/meshes/torso_v0/torso_lift_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/torso_v0/torso_lift_normals.tif
rename to trajopt_common/data/pr2/meshes/torso_v0/torso_lift_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.xml b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.xml
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex.xml
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stla b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stla
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.xml b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.xml
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll.xml
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.xml b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.xml
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L.xml
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stla b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stla
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stlb b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stlb
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_L_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stla b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stla
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stlb b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stlb
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/forearm_roll_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.xml b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.xml
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm.xml
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm.xml
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.obj b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.obj
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.obj
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.obj
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stla b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stla
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stla
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stla
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stlb b/trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stlb
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stlb
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/convex/upper_arm_convex.stlb
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl b/trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex.stl
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex.stl
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex_color.tif b/trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex_color.tif
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex_color.tif
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex_normals.tif b/trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/elbow_flex_normals.tif
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/elbow_flex_normals.tif
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl b/trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll.stl
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll.stl
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl b/trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/forearm_roll_L.stl
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/iv/elbow_flex.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/iv/elbow_flex.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/iv/elbow_flex.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/iv/elbow_flex.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/iv/forearm_roll.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/iv/forearm_roll.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/iv/forearm_roll.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/iv/forearm_roll.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/iv/forearm_roll_L.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/iv/forearm_roll_L.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/iv/forearm_roll_L.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/iv/forearm_roll_L.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/iv/upper_arm.iv b/trajopt_common/data/pr2/meshes/upper_arm_v0/iv/upper_arm.iv
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/iv/upper_arm.iv
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/iv/upper_arm.iv
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae b/trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.dae
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.dae
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.jpg b/trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.jpg
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.jpg
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.jpg
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl b/trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm.stl
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm.stl
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm_color.tif b/trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm_color.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm_color.tif
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm_color.tif
diff --git a/trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm_normals.tif b/trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm_normals.tif
similarity index 100%
rename from trajopt/test/data/pr2/meshes/upper_arm_v0/upper_arm_normals.tif
rename to trajopt_common/data/pr2/meshes/upper_arm_v0/upper_arm_normals.tif
diff --git a/trajopt/test/data/pr2_plugins.yaml b/trajopt_common/data/pr2_plugins.yaml
similarity index 100%
rename from trajopt/test/data/pr2_plugins.yaml
rename to trajopt_common/data/pr2_plugins.yaml
diff --git a/trajopt/test/data/spherebot.srdf b/trajopt_common/data/spherebot.srdf
similarity index 73%
rename from trajopt/test/data/spherebot.srdf
rename to trajopt_common/data/spherebot.srdf
index 66b5ffbc..cf87abc3 100644
--- a/trajopt/test/data/spherebot.srdf
+++ b/trajopt_common/data/spherebot.srdf
@@ -5,7 +5,7 @@
     </group>
 
     <!--Contact Managers plugins -->
-    <contact_managers_plugin_config filename="package://trajopt/test/data/contact_manager_plugins.yaml"/>
+    <contact_managers_plugin_config filename="package://trajopt_common/data/contact_manager_plugins.yaml"/>
 
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="base_link" />
 </robot>
diff --git a/trajopt/test/data/spherebot.urdf b/trajopt_common/data/spherebot.urdf
similarity index 100%
rename from trajopt/test/data/spherebot.urdf
rename to trajopt_common/data/spherebot.urdf
diff --git a/trajopt_ifopt/test/CMakeLists.txt b/trajopt_ifopt/test/CMakeLists.txt
index 9be566e7..744ab995 100644
--- a/trajopt_ifopt/test/CMakeLists.txt
+++ b/trajopt_ifopt/test/CMakeLists.txt
@@ -1,5 +1,4 @@
 find_package(GTest REQUIRED)
-find_package(trajopt REQUIRED)
 
 if(NOT TARGET GTest::GTest)
   add_library(GTest::GTest INTERFACE IMPORTED)
@@ -47,7 +46,7 @@ macro(add_gtest test_name test_file)
     GTest::Main
     ${Boost_SYSTEM_LIBRARY}
     ${Boost_PROGRAM_OPTIONS_LIBRARY}
-    trajopt::trajopt_test_data)
+    trajopt::trajopt_common)
   target_include_directories(${test_name} PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
                                                   ${GTEST_INCLUDE_DIRS})
   target_include_directories(${test_name} SYSTEM PRIVATE ${PCL_INCLUDE_DIRS})
diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
index 17b799a1..10456dd8 100644
--- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
+++ b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp
@@ -10,7 +10,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_ifopt/constraints/cartesian_line_constraint.h>
 #include <trajopt_ifopt/utils/numeric_differentiation.h>
-#include <trajopt_test_utils.hpp>
 
 using namespace trajopt_ifopt;
 using namespace std;
@@ -39,9 +38,9 @@ class CartesianLineConstraintUnit : public testing::TestWithParam<const char*>
   void SetUp() override
   {
     // Initialize Tesseract
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
-    auto locator = std::make_shared<TrajOptSupportResourceLocator>();
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
+    auto locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     bool status = env->init(urdf_file, srdf_file, locator);
     EXPECT_TRUE(status);
 
diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
index 218a4c85..fd5df119 100644
--- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
+++ b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp
@@ -40,7 +40,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_common/utils.hpp>
 #include <trajopt_ifopt/constraints/cartesian_position_constraint.h>
 #include <trajopt_ifopt/utils/numeric_differentiation.h>
-#include <trajopt_test_utils.hpp>
 
 using namespace trajopt_ifopt;
 using namespace std;
@@ -66,9 +65,9 @@ class CartesianPositionConstraintUnit : public testing::TestWithParam<const char
   void SetUp() override
   {
     // Initialize Tesseract
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
-    auto locator = std::make_shared<TrajOptSupportResourceLocator>();
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
+    auto locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     auto env = std::make_shared<Environment>();
     bool status = env->init(urdf_file, srdf_file, locator);
     EXPECT_TRUE(status);
diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp
index 4f46f978..8c174fab 100644
--- a/trajopt_ifopt/test/cast_cost_unit.cpp
+++ b/trajopt_ifopt/test/cast_cost_unit.cpp
@@ -35,7 +35,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <ifopt/ipopt_solver.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
@@ -64,9 +63,9 @@ class CastTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp
index b182dff0..28b1cd11 100644
--- a/trajopt_ifopt/test/collision_unit.cpp
+++ b/trajopt_ifopt/test/collision_unit.cpp
@@ -36,7 +36,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_ifopt/constraints/collision/discrete_collision_constraint.h>
-#include "trajopt_test_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -57,10 +56,10 @@ class CollisionUnit : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Set up collision evaluator
diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp
index ab952e69..ad530e48 100644
--- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp
+++ b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp
@@ -35,7 +35,6 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <ifopt/ipopt_solver.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_test_utils.hpp>
 #include <trajopt_common/config.hpp>
 #include <trajopt_common/eigen_conversions.hpp>
 #include <trajopt_common/logging.hpp>
@@ -65,10 +64,10 @@ class ContinuousCollisionGradientTest : public testing::TestWithParam<const char
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     gLogLevel = trajopt_common::LevelError;
diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp
index a1d61d42..bd6c8e5a 100644
--- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp
+++ b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp
@@ -43,7 +43,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h>
 #include <trajopt_ifopt/constraints/joint_position_constraint.h>
 #include <trajopt_ifopt/costs/squared_cost.h>
-#include "trajopt_test_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -62,10 +61,10 @@ class DiscreteCollisionGradientTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
   }
 };
diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
index 8e8176d9..b05b5bc0 100644
--- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
+++ b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp
@@ -40,7 +40,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_ifopt/constraints/inverse_kinematics_constraint.h>
 #include <trajopt_ifopt/utils/numeric_differentiation.h>
-#include <trajopt_test_utils.hpp>
 
 using namespace trajopt_ifopt;
 using namespace std;
@@ -66,9 +65,9 @@ class InverseKinematicsConstraintUnit : public testing::TestWithParam<const char
   void SetUp() override
   {
     // Initialize Tesseract
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     bool status = env->init(urdf_file, srdf_file, locator);
     EXPECT_TRUE(status);
 
diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp
index 5c18e15a..41ccf5db 100644
--- a/trajopt_ifopt/test/simple_collision_unit.cpp
+++ b/trajopt_ifopt/test/simple_collision_unit.cpp
@@ -42,7 +42,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h>
 #include <trajopt_ifopt/constraints/joint_position_constraint.h>
 #include <trajopt_ifopt/costs/squared_cost.h>
-#include "trajopt_test_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -61,9 +60,9 @@ class SimpleCollisionTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
   }
 };
diff --git a/trajopt_ifopt/test/trajopt_test_utils.hpp b/trajopt_ifopt/test/trajopt_test_utils.hpp
deleted file mode 100644
index 70646103..00000000
--- a/trajopt_ifopt/test/trajopt_test_utils.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * @file trajopt_test_utils.cpp
- * @brief TrajOpt IFOPT unit test utils
- *
- * @author Levi Armstrong
- * @author Matthew Powelson
- * @date May 18, 2020
- * @version TODO
- * @bug No known bugs
- *
- * @copyright Copyright (c) 2020, Southwest Research Institute
- *
- * @par License
- * Software License Agreement (Apache License)
- * @par
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- * http://www.apache.org/licenses/LICENSE-2.0
- * @par
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <trajopt_common/macros.h>
-TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <string>
-#include <tesseract_common/resource_locator.h>
-TRAJOPT_IGNORE_WARNINGS_POP
-
-class TrajOptSupportResourceLocator : public tesseract_common::ResourceLocator
-{
-public:
-  using Ptr = std::shared_ptr<TrajOptSupportResourceLocator>;
-  using ConstPtr = std::shared_ptr<const TrajOptSupportResourceLocator>;
-
-  std::shared_ptr<tesseract_common::Resource> locateResource(const std::string& url) const override
-  {
-    std::string mod_url = url;
-    if (url.find("package://trajopt") == 0)
-    {
-      mod_url.erase(0, strlen("package://trajopt"));
-      size_t pos = mod_url.find('/');
-      if (pos == std::string::npos)
-        return nullptr;
-
-      std::string package = mod_url.substr(0, pos);
-      mod_url.erase(0, pos);
-      std::string package_path = std::string(TRAJOPT_DIR);
-
-      if (package_path.empty())
-        return nullptr;
-
-      mod_url = package_path + mod_url;
-    }
-
-    if (!tesseract_common::fs::path(mod_url).is_complete())
-      return nullptr;
-
-    return std::make_shared<tesseract_common::SimpleLocatedResource>(
-        url, mod_url, std::make_shared<TrajOptSupportResourceLocator>(*this));
-  }
-};
diff --git a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt
index 83093f2b..8ad6bdb2 100644
--- a/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt
+++ b/trajopt_optimizers/trajopt_sqp/test/CMakeLists.txt
@@ -44,7 +44,7 @@ macro(add_gtest test_name test_file)
     console_bridge
     ifopt::ifopt_ipopt
     trajopt::trajopt_ifopt
-    trajopt::trajopt_test_data
+    trajopt::trajopt_common
     GTest::GTest
     GTest::Main)
   target_include_directories(${test_name} PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt
index 3d4fd50e..43199fde 100644
--- a/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt
+++ b/trajopt_optimizers/trajopt_sqp/test/benchmarks/CMakeLists.txt
@@ -1,5 +1,4 @@
 find_package(benchmark REQUIRED)
-find_package(trajopt REQUIRED)
 
 macro(add_benchmark benchmark_name benchmark_file)
   add_executable(${benchmark_name} ${benchmark_file})
@@ -12,8 +11,7 @@ macro(add_benchmark benchmark_name benchmark_file)
     ${PROJECT_NAME}
     ifopt::ifopt_ipopt
     trajopt::trajopt_ifopt
-    trajopt::trajopt_test_data
-    trajopt::trajopt
+    trajopt::trajopt_common
     benchmark::benchmark)
   target_include_directories(${benchmark_name} PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>")
   add_dependencies(${benchmark_name} ${PROJECT_NAME})
diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp
index 9aa449fb..e053172b 100644
--- a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp
@@ -26,7 +26,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "../test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -334,10 +333,10 @@ int main(int argc, char** argv)
   //////////////////////////////////////
 
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     auto env = std::make_shared<Environment>();
     env->init(urdf_file, srdf_file, locator);
 
@@ -357,10 +356,10 @@ int main(int argc, char** argv)
   // Planning Solve
   //////////////////////////////////////
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     auto env = std::make_shared<Environment>();
     env->init(urdf_file, srdf_file, locator);
 
diff --git a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp
index 1b384238..e39675e1 100644
--- a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp
@@ -48,7 +48,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_ifopt/variable_sets/joint_position_variable.h>
 #include <trajopt_ifopt/costs/squared_cost.h>
 #include <trajopt_ifopt/utils/trajopt_utils.h>
-#include "test_suite_utils.hpp"
 
 const bool DEBUG = false;
 
@@ -65,9 +64,9 @@ class CartPositionOptimization : public testing::TestWithParam<const char*>
       console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_NONE);
 
     // 1)  Load Robot
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
-    tesseract_common::ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
+    tesseract_common::ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     env = std::make_shared<tesseract_environment::Environment>();
     env->init(urdf_file, srdf_file, locator);
   }
diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp
index 54aa93f0..f4d768fa 100644
--- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp
@@ -45,7 +45,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -64,10 +63,10 @@ class CastAttachedTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp
index 28bf802c..361fc872 100644
--- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp
@@ -47,7 +47,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -66,10 +65,10 @@ class CastOctomapTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot_world.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp
index bd5ee3dc..e12b9a0f 100644
--- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp
@@ -43,7 +43,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -62,9 +61,9 @@ class CastTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
   }
 };
diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp
index b5bc9d69..1416f311 100644
--- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp
@@ -44,7 +44,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -63,10 +62,10 @@ class CastWorldTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot_world.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/boxbot.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot_world.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/boxbot.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
diff --git a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp
index f80467ef..17121066 100644
--- a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp
@@ -44,7 +44,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -62,10 +61,10 @@ class NumericalIKTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp
index cd6279b5..0abee9b2 100644
--- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp
@@ -46,7 +46,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -63,10 +62,10 @@ class PlanningTest : public testing::TestWithParam<const char*>
   Visualization::Ptr plotter;                             /**< Trajopt Plotter */
   void SetUp() override
   {
-    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/arm_around_table.urdf");
-    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/pr2.srdf");
+    tesseract_common::fs::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/arm_around_table.urdf");
+    tesseract_common::fs::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/pr2.srdf");
 
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
 
     // Create plotting tool
diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp
index 3eae7ccc..a836f96d 100644
--- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp
+++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp
@@ -48,7 +48,6 @@ TRAJOPT_IGNORE_WARNINGS_POP
 #include <trajopt_sqp/trajopt_qp_problem.h>
 #include <trajopt_sqp/trust_region_sqp_solver.h>
 #include <trajopt_sqp/osqp_eigen_solver.h>
-#include "test_suite_utils.hpp"
 
 using namespace trajopt_ifopt;
 using namespace tesseract_environment;
@@ -201,9 +200,9 @@ class SimpleCollisionTest : public testing::TestWithParam<const char*>
 
   void SetUp() override
   {
-    boost::filesystem::path urdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.urdf");
-    boost::filesystem::path srdf_file(std::string(TRAJOPT_DIR) + "/test/data/spherebot.srdf");
-    ResourceLocator::Ptr locator = std::make_shared<TrajOptSupportResourceLocator>();
+    boost::filesystem::path urdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.urdf");
+    boost::filesystem::path srdf_file(std::string(TRAJOPT_DATA_DIR) + "/spherebot.srdf");
+    ResourceLocator::Ptr locator = std::make_shared<tesseract_common::GeneralResourceLocator>();
     EXPECT_TRUE(env->init(urdf_file, srdf_file, locator));
   }
 };
diff --git a/trajopt_optimizers/trajopt_sqp/test/test_suite_utils.hpp b/trajopt_optimizers/trajopt_sqp/test/test_suite_utils.hpp
deleted file mode 100644
index e488320b..00000000
--- a/trajopt_optimizers/trajopt_sqp/test/test_suite_utils.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * @file test_suite_utils.cpp
- * @brief test utils
- *
- * @author Levi Armstrong
- * @author Matthew Powelson
- * @date May 18, 2020
- * @version TODO
- * @bug No known bugs
- *
- * @copyright Copyright (c) 2020, Southwest Research Institute
- *
- * @par License
- * Software License Agreement (Apache License)
- * @par
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- * http://www.apache.org/licenses/LICENSE-2.0
- * @par
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include <trajopt_common/macros.h>
-TRAJOPT_IGNORE_WARNINGS_PUSH
-#include <string>
-#include <tesseract_common/resource_locator.h>
-TRAJOPT_IGNORE_WARNINGS_POP
-
-class TrajOptSupportResourceLocator : public tesseract_common::ResourceLocator
-{
-public:
-  using Ptr = std::shared_ptr<TrajOptSupportResourceLocator>;
-  using ConstPtr = std::shared_ptr<const TrajOptSupportResourceLocator>;
-
-  std::shared_ptr<tesseract_common::Resource> locateResource(const std::string& url) const override
-  {
-    std::string mod_url = url;
-    if (url.find("package://trajopt") == 0)
-    {
-      mod_url.erase(0, strlen("package://trajopt"));
-      size_t pos = mod_url.find('/');
-      if (pos == std::string::npos)
-        return nullptr;
-
-      std::string package = mod_url.substr(0, pos);
-      mod_url.erase(0, pos);
-      std::string package_path = std::string(TRAJOPT_DIR);
-
-      if (package_path.empty())
-        return nullptr;
-
-      mod_url = package_path + mod_url;
-    }
-
-    if (!tesseract_common::fs::path(mod_url).is_complete())
-      return nullptr;
-
-    return std::make_shared<tesseract_common::SimpleLocatedResource>(
-        url, mod_url, std::make_shared<TrajOptSupportResourceLocator>(*this));
-  }
-};