diff --git a/trajopt_common/CMakeLists.txt b/trajopt_common/CMakeLists.txt
index 4c058afd..caf931e9 100644
--- a/trajopt_common/CMakeLists.txt
+++ b/trajopt_common/CMakeLists.txt
@@ -15,6 +15,8 @@ find_package(Eigen3 REQUIRED)
 find_package(Boost COMPONENTS program_options REQUIRED)
 find_package(ros_industrial_cmake_boilerplate REQUIRED)
 find_package(tesseract_common REQUIRED)
+find_package(tesseract_collision COMPONENTS core REQUIRED)
+find_package(tesseract_kinematics COMPONENTS core REQUIRED)
 
 # Load variable for clang tidy args, compiler options and cxx version
 trajopt_variables()
@@ -22,12 +24,20 @@ trajopt_variables()
 set(UTILS_SOURCE_FILES
     src/stl_to_string.cpp
     src/clock.cpp
+    src/collision_types.cpp
+    src/collision_utils.cpp
     src/config.cpp
     src/logging.cpp
     src/utils.cpp)
 
 add_library(${PROJECT_NAME} ${UTILS_SOURCE_FILES})
-target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen Boost::program_options tesseract::tesseract_common)
+target_link_libraries(
+  ${PROJECT_NAME}
+  PUBLIC Eigen3::Eigen
+         Boost::program_options
+         tesseract::tesseract_common
+         tesseract::tesseract_collision_core
+         tesseract::tesseract_kinematics_core)
 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}
diff --git a/trajopt_common/cmake/trajopt_common-config.cmake.in b/trajopt_common/cmake/trajopt_common-config.cmake.in
index 0277eb7a..8c53f908 100644
--- a/trajopt_common/cmake/trajopt_common-config.cmake.in
+++ b/trajopt_common/cmake/trajopt_common-config.cmake.in
@@ -8,8 +8,12 @@ include(CMakeFindDependencyMacro)
 find_dependency(Eigen3)
 if(${CMAKE_VERSION} VERSION_LESS "3.15.0")
     find_package(Boost COMPONENTS program_options REQUIRED)
+    find_package(tesseract_collision COMPONENTS core REQUIRED)
+    find_package(tesseract_kinematics COMPONENTS core REQUIRED)
 else()
     find_dependency(Boost COMPONENTS program_options)
+    find_dependency(tesseract_collision COMPONENTS core)
+    find_dependency(tesseract_kinematics COMPONENTS core)
 endif()
 find_dependency(tesseract_common)
 
diff --git a/trajopt_ifopt/include/trajopt_ifopt/cache.h b/trajopt_common/include/trajopt_common/cache.h
similarity index 85%
rename from trajopt_ifopt/include/trajopt_ifopt/cache.h
rename to trajopt_common/include/trajopt_common/cache.h
index 4a4fefac..644170c1 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/cache.h
+++ b/trajopt_common/include/trajopt_common/cache.h
@@ -1,5 +1,5 @@
-#ifndef TRAJOPT_IFOPT_CACHE_H
-#define TRAJOPT_IFOPT_CACHE_H
+#ifndef TRAJOPT_COMMON_CACHE_H
+#define TRAJOPT_COMMON_CACHE_H
 
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
@@ -9,7 +9,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <memory>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-namespace trajopt_ifopt
+namespace trajopt_common
 {
 template <typename KeyT, class ValueT>
 class Cache
@@ -41,5 +41,5 @@ class Cache
   std::vector<ValueT> valbuf_;  // circular buffer
 };
 
-}  // namespace trajopt_ifopt
-#endif  // TRAJOPT_IFOPT_CACHE_H
+}  // namespace trajopt_common
+#endif  // TRAJOPT_COMMON_CACHE_H
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h
similarity index 96%
rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h
rename to trajopt_common/include/trajopt_common/collision_types.h
index 149bff8c..e67c818c 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h
+++ b/trajopt_common/include/trajopt_common/collision_types.h
@@ -21,8 +21,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TRAJOPT_IFOPT_COLLISION_TYPES_H
-#define TRAJOPT_IFOPT_COLLISION_TYPES_H
+#ifndef TRAJOPT_COMMON_COLLISION_TYPES_H
+#define TRAJOPT_COMMON_COLLISION_TYPES_H
 
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
@@ -30,13 +30,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <array>
 #include <memory>
 #include <tesseract_collision/core/types.h>
-#include <tesseract_environment/environment.h>
-#include <ifopt/composite.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/cache.h>
+#include <trajopt_common/cache.h>
 
-namespace trajopt_ifopt
+namespace trajopt_common
 {
 using GetStateFn = std::function<tesseract_common::TransformMap(const Eigen::Ref<const Eigen::VectorXd>& joint_values)>;
 
@@ -266,5 +264,5 @@ struct CollisionCacheData
 
 using CollisionCache = Cache<size_t, CollisionCacheData::ConstPtr>;
 
-}  // namespace trajopt_ifopt
-#endif  // TRAJOPT_IFOPT_COLLISION_TYPES_H
+}  // namespace trajopt_common
+#endif  // TRAJOPT_COMMON_COLLISION_TYPES_H
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h b/trajopt_common/include/trajopt_common/collision_utils.h
similarity index 94%
rename from trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h
rename to trajopt_common/include/trajopt_common/collision_utils.h
index 28e5bba4..9af20305 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h
+++ b/trajopt_common/include/trajopt_common/collision_utils.h
@@ -21,8 +21,8 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
-#ifndef TRAJOPT_IFOPT_COLLISION_UTILS_H
-#define TRAJOPT_IFOPT_COLLISION_UTILS_H
+#ifndef TRAJOPT_COMMON_COLLISION_UTILS_H
+#define TRAJOPT_COMMON_COLLISION_UTILS_H
 
 #include <trajopt_common/macros.h>
 TRAJOPT_IGNORE_WARNINGS_PUSH
@@ -31,9 +31,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <tesseract_kinematics/core/joint_group.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
+#include <trajopt_common/collision_types.h>
 
-namespace trajopt_ifopt
+namespace trajopt_common
 {
 std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref<const Eigen::VectorXd>& dof_vals);
 std::size_t getHash(const TrajOptCollisionConfig& collision_config,
@@ -94,5 +94,5 @@ void debugPrintInfo(const tesseract_collision::ContactResult& res,
                     const Eigen::VectorXd& dof_vals,
                     bool header = false);
 
-}  // namespace trajopt_ifopt
-#endif  // TRAJOPT_IFOPT_COLLISION_UTILS_H
+}  // namespace trajopt_common
+#endif  // TRAJOPT_COMMON_COLLISION_UTILS_H
diff --git a/trajopt_common/package.xml b/trajopt_common/package.xml
index 0c1a4222..218f5dcf 100644
--- a/trajopt_common/package.xml
+++ b/trajopt_common/package.xml
@@ -18,6 +18,8 @@
   <exec_depend>libboost-program-options</exec_depend>
 
   <depend>tesseract_common</depend>
+  <depend>tesseract_collision</depend>
+  <depend>tesseract_kinematics</depend>
 
   <export>
     <build_type>cmake</build_type>
diff --git a/trajopt_ifopt/src/constraints/collision/collision_types.cpp b/trajopt_common/src/collision_types.cpp
similarity index 98%
rename from trajopt_ifopt/src/constraints/collision/collision_types.cpp
rename to trajopt_common/src/collision_types.cpp
index 0140fc53..00b6a3cd 100644
--- a/trajopt_ifopt/src/constraints/collision/collision_types.cpp
+++ b/trajopt_common/src/collision_types.cpp
@@ -22,9 +22,9 @@
  * limitations under the License.
  */
 
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
+#include <trajopt_common/collision_types.h>
 
-namespace trajopt_ifopt
+namespace trajopt_common
 {
 CollisionCoeffData::CollisionCoeffData(double default_collision_coeff)
   : default_collision_coeff_(default_collision_coeff)
@@ -260,4 +260,4 @@ double GradientResultsSet::getMaxErrorWithBufferT1() const
   return e;
 }
 
-}  // namespace trajopt_ifopt
+}  // namespace trajopt_common
diff --git a/trajopt_ifopt/src/constraints/collision/collision_utils.cpp b/trajopt_common/src/collision_utils.cpp
similarity index 98%
rename from trajopt_ifopt/src/constraints/collision/collision_utils.cpp
rename to trajopt_common/src/collision_utils.cpp
index 2442e341..c4c7265a 100644
--- a/trajopt_ifopt/src/constraints/collision/collision_utils.cpp
+++ b/trajopt_common/src/collision_utils.cpp
@@ -26,13 +26,12 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <boost/functional/hash.hpp>
 #include <console_bridge/console.h>
-#include <tesseract_kinematics/core/joint_group.h>
 #include <tesseract_kinematics/core/utils.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
+#include <trajopt_common/collision_utils.h>
 
-namespace trajopt_ifopt
+namespace trajopt_common
 {
 std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref<const Eigen::VectorXd>& dof_vals)
 {
@@ -318,4 +317,4 @@ void debugPrintInfo(const tesseract_collision::ContactResult& res,
 
   std::printf("\n");
 }
-}  // namespace trajopt_ifopt
+}  // namespace trajopt_common
diff --git a/trajopt_ifopt/CMakeLists.txt b/trajopt_ifopt/CMakeLists.txt
index 0ed354c1..ce04396d 100644
--- a/trajopt_ifopt/CMakeLists.txt
+++ b/trajopt_ifopt/CMakeLists.txt
@@ -46,9 +46,7 @@ set(TRAJOPT_IFOPT_SOURCE_FILES
     src/constraints/joint_jerk_constraint.cpp
     src/constraints/joint_position_constraint.cpp
     src/constraints/joint_velocity_constraint.cpp
-    src/constraints/collision/collision_types.cpp
     src/constraints/cartesian_line_constraint.cpp
-    src/constraints/collision/collision_utils.cpp
     src/constraints/collision/discrete_collision_evaluators.cpp
     src/constraints/collision/continuous_collision_evaluators.cpp
     src/constraints/collision/weighted_average_methods.cpp
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
index 193b69f1..31f5aa66 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h
@@ -35,7 +35,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <tesseract_kinematics/core/joint_group.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
+#include <trajopt_common/collision_types.h>
 #include <trajopt_ifopt/variable_sets/joint_position_variable.h>
 
 namespace trajopt_ifopt
@@ -66,10 +66,11 @@ class ContinuousCollisionEvaluator
    * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the
    * data.
    */
-  virtual CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                                         const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                                         const std::array<bool, 2>& position_vars_fixed,
-                                                         std::size_t bounds_size) = 0;
+  virtual trajopt_common::CollisionCacheData::ConstPtr
+  CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                    const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                    const std::array<bool, 2>& position_vars_fixed,
+                    std::size_t bounds_size) = 0;
 
   /**
    * @brief Extracts the gradient information based on the contact results for the transition between dof_vals0 and
@@ -80,15 +81,16 @@ class ContinuousCollisionEvaluator
    * data
    * @return The gradient results
    */
-  virtual GradientResults CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                           const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                           const tesseract_collision::ContactResult& contact_results) = 0;
+  virtual trajopt_common::GradientResults
+  CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                   const tesseract_collision::ContactResult& contact_results) = 0;
 
   /**
    * @brief Get the safety margin information.
    * @return Safety margin information
    */
-  virtual const TrajOptCollisionConfig& GetCollisionConfig() const = 0;
+  virtual const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const = 0;
 };
 
 /**
@@ -103,37 +105,39 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator
   using Ptr = std::shared_ptr<LVSContinuousCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const LVSContinuousCollisionEvaluator>;
 
-  LVSContinuousCollisionEvaluator(std::shared_ptr<CollisionCache> collision_cache,
+  LVSContinuousCollisionEvaluator(std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
                                   tesseract_kinematics::JointGroup::ConstPtr manip,
                                   tesseract_environment::Environment::ConstPtr env,
-                                  TrajOptCollisionConfig::ConstPtr collision_config,
+                                  trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
                                   bool dynamic_environment = false);
 
-  CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                                 const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                                 const std::array<bool, 2>& position_vars_fixed,
-                                                 std::size_t bounds_size) override final;
+  trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                                                                 const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                                                                 const std::array<bool, 2>& position_vars_fixed,
+                                                                 std::size_t bounds_size) override final;
 
-  GradientResults CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                   const tesseract_collision::ContactResult& contact_results) override final;
+  trajopt_common::GradientResults
+  CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                   const tesseract_collision::ContactResult& contact_results) override final;
 
-  const TrajOptCollisionConfig& GetCollisionConfig() const override final;
+  const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final;
 
 private:
-  std::shared_ptr<CollisionCache> collision_cache_;
+  std::shared_ptr<trajopt_common::CollisionCache> collision_cache_;
   tesseract_kinematics::JointGroup::ConstPtr manip_;
   tesseract_environment::Environment::ConstPtr env_;
-  TrajOptCollisionConfig::ConstPtr collision_config_;
+  trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_;
   std::vector<std::string> env_active_link_names_;
   std::vector<std::string> manip_active_link_names_;
   std::vector<std::string> diff_active_link_names_;
-  GetStateFn get_state_fn_;
+  trajopt_common::GetStateFn get_state_fn_;
   bool dynamic_environment_;
   tesseract_collision::ContinuousContactManager::Ptr contact_manager_;
 
-  CollisionCacheData::ConstPtr CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                                             const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
+  trajopt_common::CollisionCacheData::ConstPtr
+  CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                                const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
 
   void CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                             const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
@@ -152,37 +156,39 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator
   using Ptr = std::shared_ptr<LVSDiscreteCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const LVSDiscreteCollisionEvaluator>;
 
-  LVSDiscreteCollisionEvaluator(std::shared_ptr<CollisionCache> collision_cache,
+  LVSDiscreteCollisionEvaluator(std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
                                 tesseract_kinematics::JointGroup::ConstPtr manip,
                                 tesseract_environment::Environment::ConstPtr env,
-                                TrajOptCollisionConfig::ConstPtr collision_config,
+                                trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
                                 bool dynamic_environment = false);
 
-  CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                                 const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                                 const std::array<bool, 2>& position_vars_fixed,
-                                                 std::size_t bounds_size) override final;
+  trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                                                                 const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                                                                 const std::array<bool, 2>& position_vars_fixed,
+                                                                 std::size_t bounds_size) override final;
 
-  GradientResults CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
-                                   const tesseract_collision::ContactResult& contact_results) override final;
+  trajopt_common::GradientResults
+  CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
+                   const tesseract_collision::ContactResult& contact_results) override final;
 
-  const TrajOptCollisionConfig& GetCollisionConfig() const override final;
+  const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final;
 
 private:
-  std::shared_ptr<CollisionCache> collision_cache_;
+  std::shared_ptr<trajopt_common::CollisionCache> collision_cache_;
   tesseract_kinematics::JointGroup::ConstPtr manip_;
   tesseract_environment::Environment::ConstPtr env_;
-  TrajOptCollisionConfig::ConstPtr collision_config_;
+  trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_;
   std::vector<std::string> env_active_link_names_;
   std::vector<std::string> manip_active_link_names_;
   std::vector<std::string> diff_active_link_names_;
-  GetStateFn get_state_fn_;
+  trajopt_common::GetStateFn get_state_fn_;
   bool dynamic_environment_;
   tesseract_collision::DiscreteContactManager::Ptr contact_manager_;
 
-  CollisionCacheData::ConstPtr CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
-                                                             const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
+  trajopt_common::CollisionCacheData::ConstPtr
+  CalcCollisionsCacheDataHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
+                                const Eigen::Ref<const Eigen::VectorXd>& dof_vals1);
 
   void CalcCollisionsHelper(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                             const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
index 5b804361..b355efef 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h
@@ -34,7 +34,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <tesseract_kinematics/core/joint_group.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
+#include <trajopt_common/collision_types.h>
 #include <trajopt_ifopt/variable_sets/joint_position_variable.h>
 
 namespace trajopt_ifopt
@@ -64,14 +64,14 @@ class DiscreteCollisionEvaluator
    * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the
    * data.
    */
-  virtual CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
-                                                      std::size_t bounds_size) = 0;
+  virtual trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
+                                                                      std::size_t bounds_size) = 0;
 
   /**
    * @brief Get the safety margin information.
    * @return Safety margin information
    */
-  virtual const TrajOptCollisionConfig& GetCollisionConfig() const = 0;
+  virtual const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const = 0;
 
   /**
    * @brief Extracts the gradient information based on the contact results
@@ -80,8 +80,8 @@ class DiscreteCollisionEvaluator
    * @param isTimestep1 Indicates if this is the second timestep when computing gradient for continuous collision
    * @return The gradient results
    */
-  virtual GradientResults GetGradient(const Eigen::VectorXd& dofvals,
-                                      const tesseract_collision::ContactResult& contact_result) = 0;
+  virtual trajopt_common::GradientResults GetGradient(const Eigen::VectorXd& dofvals,
+                                                      const tesseract_collision::ContactResult& contact_result) = 0;
 };
 
 /**
@@ -96,29 +96,29 @@ class SingleTimestepCollisionEvaluator : public DiscreteCollisionEvaluator
   using Ptr = std::shared_ptr<SingleTimestepCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const SingleTimestepCollisionEvaluator>;
 
-  SingleTimestepCollisionEvaluator(std::shared_ptr<CollisionCache> collision_cache,
+  SingleTimestepCollisionEvaluator(std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
                                    tesseract_kinematics::JointGroup::ConstPtr manip,
                                    tesseract_environment::Environment::ConstPtr env,
-                                   TrajOptCollisionConfig::ConstPtr collision_config,
+                                   trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
                                    bool dynamic_environment = false);
 
-  CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
-                                              std::size_t bounds_size) override final;
+  trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
+                                                              std::size_t bounds_size) override final;
 
-  GradientResults GetGradient(const Eigen::VectorXd& dofvals,
-                              const tesseract_collision::ContactResult& contact_result) override;
+  trajopt_common::GradientResults GetGradient(const Eigen::VectorXd& dofvals,
+                                              const tesseract_collision::ContactResult& contact_result) override;
 
-  const TrajOptCollisionConfig& GetCollisionConfig() const override;
+  const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override;
 
 private:
-  std::shared_ptr<CollisionCache> collision_cache_;
+  std::shared_ptr<trajopt_common::CollisionCache> collision_cache_;
   tesseract_kinematics::JointGroup::ConstPtr manip_;
   tesseract_environment::Environment::ConstPtr env_;
-  TrajOptCollisionConfig::ConstPtr collision_config_;
+  trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_;
   std::vector<std::string> env_active_link_names_;
   std::vector<std::string> manip_active_link_names_;
   std::vector<std::string> diff_active_link_names_;
-  GetStateFn get_state_fn_;
+  trajopt_common::GetStateFn get_state_fn_;
   bool dynamic_environment_;
   tesseract_collision::DiscreteContactManager::Ptr contact_manager_;
 
diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h
index 49637e3c..62a0fd54 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h
@@ -29,14 +29,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigen>
 TRAJOPT_IGNORE_WARNINGS_POP
 
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
+#include <trajopt_common/collision_types.h>
 
 namespace trajopt_ifopt
 {
-Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_set,
+Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSet& grad_results_set,
                                          double max_error_with_buffer,
                                          Eigen::Index size);
-Eigen::VectorXd getWeightedAvgGradientT1(const GradientResultsSet& grad_results_set,
+Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSet& grad_results_set,
                                          double max_error_with_buffer,
                                          Eigen::Index size);
 }  // namespace trajopt_ifopt
diff --git a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h b/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h
index 24b6b7da..535e1d28 100644
--- a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h
+++ b/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h
@@ -28,8 +28,6 @@
 #include <trajopt_ifopt/constraints/collision/continuous_collision_constraint.h>
 #include <trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h>
 #include <trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h>
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
-#include <trajopt_ifopt/constraints/collision/collision_types.h>
 #include <trajopt_ifopt/constraints/collision/weighted_average_methods.h>
 
 #include <trajopt_ifopt/constraints/cartesian_position_constraint.h>
diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp
index 0c59026e..9a4b1957 100644
--- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp
@@ -87,7 +87,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const
   double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer;
   Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast<Eigen::Index>(bounds_.size()), -margin_buffer);
 
-  CollisionCacheData::ConstPtr collision_data =
+  auto collision_data =
       collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size());
 
   if (collision_data->gradient_results_sets.empty())
@@ -98,7 +98,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxError();
     }
   }
@@ -106,7 +106,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       if (r.max_error[0].has_error[0] || r.max_error[1].has_error[0])
         values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxErrorT0();
     }
@@ -115,7 +115,7 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       if (r.max_error[0].has_error[1] || r.max_error[1].has_error[1])
         values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxErrorT1();
     }
@@ -141,7 +141,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob
   Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues();
   Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues();
 
-  CollisionCacheData::ConstPtr collision_data =
+  auto collision_data =
       collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size());
 
   if (collision_data->gradient_results_sets.empty())
@@ -153,7 +153,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       if (r.max_error[0].has_error[0] || r.max_error[1].has_error[0])
       {
         double max_error_with_buffer = r.getMaxErrorWithBufferT0();
@@ -172,7 +172,7 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       if (r.max_error[0].has_error[1] || r.max_error[1].has_error[1])
       {
         double max_error_with_buffer = r.getMaxErrorWithBufferT1();
diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
index c6b090ab..5dbda07f 100644
--- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
+++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp
@@ -23,15 +23,15 @@
  */
 
 #include <trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h>
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
+#include <trajopt_common/collision_utils.h>
 
 namespace trajopt_ifopt
 {
 LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator(
-    std::shared_ptr<CollisionCache> collision_cache,
+    std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
     tesseract_kinematics::JointGroup::ConstPtr manip,
     tesseract_environment::Environment::ConstPtr env,
-    trajopt_ifopt::TrajOptCollisionConfig::ConstPtr collision_config,
+    trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
     bool dynamic_environment)
   : collision_cache_(std::move(collision_cache))
   , manip_(std::move(manip))
@@ -39,9 +39,8 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator(
   , collision_config_(std::move(collision_config))
   , dynamic_environment_(dynamic_environment)
 {
-  if (collision_config_->type != tesseract_collision::CollisionEvaluatorType::CONTINUOUS &&
-      collision_config_->type != tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS)
-    throw std::runtime_error("LVSContinuousCollisionEvaluator, type must be CONTINUOUS or LVS_CONTINUOUS");
+  assert(collision_config_->type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS ||
+         collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS);
 
   manip_active_link_names_ = manip_->getActiveLinkNames();
 
@@ -78,13 +77,13 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator(
       collision_config_->collision_margin_buffer);
 }
 
-CollisionCacheData::ConstPtr
+trajopt_common::CollisionCacheData::ConstPtr
 LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                                                    const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                                                    const std::array<bool, 2>& position_vars_fixed,
                                                    std::size_t bounds_size)
 {
-  size_t key = getHash(*collision_config_, dof_vals0, dof_vals1);
+  size_t key = trajopt_common::getHash(*collision_config_, dof_vals0, dof_vals1);
   auto* it = collision_cache_->get(key);
   if (it != nullptr)
   {
@@ -92,24 +91,24 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
     return *it;
   }
 
-  auto data = std::make_shared<CollisionCacheData>();
+  auto data = std::make_shared<trajopt_common::CollisionCacheData>();
   CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map);
 
   for (const auto& pair : data->contact_results_map)
   {
-    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, GradientResultsSet>;
+    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, trajopt_common::GradientResultsSet>;
     ShapeGrsType shape_grs;
     const double coeff =
         collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second);
     for (const tesseract_collision::ContactResult& dist_result : pair.second)
     {
-      const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
-      const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
+      const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
+      const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
       auto shape_key = std::make_pair(shape_hash0, shape_hash1);
       auto it = shape_grs.find(shape_key);
       if (it == shape_grs.end())
       {
-        GradientResultsSet grs;
+        trajopt_common::GradientResultsSet grs;
         grs.key = pair.first;
         grs.shape_key = shape_key;
         grs.coeff = coeff;
@@ -140,7 +139,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer();
                 });
     }
@@ -148,7 +147,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0();
                 });
     }
@@ -156,7 +155,7 @@ LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen:
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1();
                 });
     }
@@ -200,7 +199,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<cons
                                                                                                pair.first.second);
     double coeff = collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second);
     const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff };
-    removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */
+    trajopt_common::removeInvalidContactResults(pair.second, data); /** @todo Should this be removed? levi */
   };
 
   if (collision_config_->type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS &&
@@ -248,7 +247,7 @@ void LVSContinuousCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<cons
   }
 }
 
-GradientResults
+trajopt_common::GradientResults
 LVSContinuousCollisionEvaluator::CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                                                   const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                                                   const tesseract_collision::ContactResult& contact_results)
@@ -257,10 +256,11 @@ LVSContinuousCollisionEvaluator::CalcGradientData(const Eigen::Ref<const Eigen::
   double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(
       contact_results.link_names[0], contact_results.link_names[1]);
 
-  return getGradient(dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_);
+  return trajopt_common::getGradient(
+      dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_);
 }
 
-const trajopt_ifopt::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::GetCollisionConfig() const
+const trajopt_common::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::GetCollisionConfig() const
 {
   return *collision_config_;
 }
@@ -268,10 +268,10 @@ const trajopt_ifopt::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::Ge
 //////////////////////////////////////////
 
 LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator(
-    std::shared_ptr<CollisionCache> collision_cache,
+    std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
     tesseract_kinematics::JointGroup::ConstPtr manip,
     tesseract_environment::Environment::ConstPtr env,
-    trajopt_ifopt::TrajOptCollisionConfig::ConstPtr collision_config,
+    trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
     bool dynamic_environment)
   : collision_cache_(std::move(collision_cache))
   , manip_(std::move(manip))
@@ -314,7 +314,7 @@ LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator(
       collision_config_->collision_margin_buffer);
 }
 
-CollisionCacheData::ConstPtr
+trajopt_common::CollisionCacheData::ConstPtr
 LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                                                  const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                                                  const std::array<bool, 2>& position_vars_fixed,
@@ -328,23 +328,23 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
     return *it;
   }
 
-  auto data = std::make_shared<CollisionCacheData>();
+  auto data = std::make_shared<trajopt_common::CollisionCacheData>();
   CalcCollisionsHelper(dof_vals0, dof_vals1, data->contact_results_map);
   for (const auto& pair : data->contact_results_map)
   {
-    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, GradientResultsSet>;
+    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, trajopt_common::GradientResultsSet>;
     ShapeGrsType shape_grs;
     const double coeff =
         collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second);
     for (const tesseract_collision::ContactResult& dist_result : pair.second)
     {
-      const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
-      const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
+      const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
+      const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
       auto shape_key = std::make_pair(shape_hash0, shape_hash1);
       auto it = shape_grs.find(shape_key);
       if (it == shape_grs.end())
       {
-        GradientResultsSet grs;
+        trajopt_common::GradientResultsSet grs;
         grs.key = pair.first;
         grs.shape_key = shape_key;
         grs.coeff = coeff;
@@ -375,7 +375,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBuffer() > b.getMaxErrorWithBuffer();
                 });
     }
@@ -383,7 +383,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBufferT0() > b.getMaxErrorWithBufferT0();
                 });
     }
@@ -391,7 +391,7 @@ LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref<const Eigen::V
     {
       std::sort(data->gradient_results_sets.begin(),
                 data->gradient_results_sets.end(),
-                [](const GradientResultsSet& a, const GradientResultsSet& b) {
+                [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                   return a.getMaxErrorWithBufferT1() > b.getMaxErrorWithBufferT1();
                 });
     }
@@ -432,7 +432,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const
     const Eigen::Vector3d data = { dist, collision_config_->collision_margin_buffer, coeff };
 
     // Don't include contacts at the fixed state
-    removeInvalidContactResults(pair.second, data);
+    trajopt_common::removeInvalidContactResults(pair.second, data);
   };
 
   // The first step is to see if the distance between two states is larger than the longest valid segment. If larger
@@ -474,7 +474,7 @@ void LVSDiscreteCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<const
   }
 }
 
-GradientResults
+trajopt_common::GradientResults
 LVSDiscreteCollisionEvaluator::CalcGradientData(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                                                 const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                                                 const tesseract_collision::ContactResult& contact_results)
@@ -483,10 +483,11 @@ LVSDiscreteCollisionEvaluator::CalcGradientData(const Eigen::Ref<const Eigen::Ve
   double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(
       contact_results.link_names[0], contact_results.link_names[1]);
 
-  return getGradient(dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_);
+  return trajopt_common::getGradient(
+      dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, manip_);
 }
 
-const trajopt_ifopt::TrajOptCollisionConfig& LVSDiscreteCollisionEvaluator::GetCollisionConfig() const
+const trajopt_common::TrajOptCollisionConfig& LVSDiscreteCollisionEvaluator::GetCollisionConfig() const
 {
   return *collision_config_;
 }
diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp
index c0ff46dd..8a25f054 100644
--- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp
@@ -88,7 +88,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const
   double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer;
   Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast<Eigen::Index>(bounds_.size()), -margin_buffer);
 
-  CollisionCacheData::ConstPtr collision_data =
+  auto collision_data =
       collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size());
 
   if (collision_data->gradient_results_sets.empty())
@@ -100,7 +100,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxError();
     }
   }
@@ -108,7 +108,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxErrorT0();
     }
   }
@@ -116,7 +116,7 @@ Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const
   {
     for (std::size_t i = 0; i < cnt; ++i)
     {
-      const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+      const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
       values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxErrorT1();
     }
   }
@@ -143,7 +143,7 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s
   Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues();
   Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues();
 
-  CollisionCacheData::ConstPtr collision_data =
+  auto collision_data =
       collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, position_vars_fixed_, bounds_.size());
   if (collision_data->gradient_results_sets.empty())
     return;
@@ -155,13 +155,13 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s
   for (int j = 0; j < n_dof_; j++)
   {
     jv(j) = joint_vals0(j) + delta;
-    CollisionCacheData::ConstPtr collision_data_delta =
+    auto collision_data_delta =
         collision_evaluator_->CalcCollisionData(jv, joint_vals1, position_vars_fixed_, bounds_.size());
 
     for (int i = 0; i < static_cast<int>(cnt); ++i)
     {
-      const GradientResultsSet& baseline = collision_data->gradient_results_sets[static_cast<std::size_t>(i)];
-      auto fn = [&baseline](const GradientResultsSet& cr) {
+      const auto& baseline = collision_data->gradient_results_sets[static_cast<std::size_t>(i)];
+      auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) {
         return (cr.key == baseline.key && cr.shape_key == baseline.shape_key);
       };
       auto it = std::find_if(
diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp
index d7a173bf..b8c95e61 100644
--- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp
@@ -31,8 +31,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 TRAJOPT_IGNORE_WARNINGS_POP
 
+#include <trajopt_common/collision_utils.h>
 #include <trajopt_ifopt/constraints/collision/discrete_collision_constraint.h>
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
 #include <trajopt_ifopt/constraints/collision/weighted_average_methods.h>
 #include <trajopt_ifopt/utils/numeric_differentiation.h>
 
@@ -93,7 +93,8 @@ void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobia
 Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref<const Eigen::VectorXd>& joint_vals) const
 {
   // Check the collisions
-  CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
+  trajopt_common::CollisionCacheData::ConstPtr collision_data =
+      collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
   double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer;
   Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast<Eigen::Index>(bounds_.size()), -margin_buffer);
 
@@ -103,7 +104,7 @@ Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref<const E
   const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size());
   for (std::size_t i = 0; i < cnt; ++i)
   {
-    const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+    const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
     values(static_cast<Eigen::Index>(i)) = r.coeff * r.getMaxErrorT0();
   }
 
@@ -123,7 +124,8 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Ref<const Eigen
   if (!triplet_list_.empty())                                               // NOLINT
     jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end());  // NOLINT
 
-  CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
+  trajopt_common::CollisionCacheData::ConstPtr collision_data =
+      collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
   if (collision_data->gradient_results_sets.empty())
     return;
 
@@ -131,7 +133,7 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Ref<const Eigen
   const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size());
   for (std::size_t i = 0; i < cnt; ++i)
   {
-    const GradientResultsSet& r = collision_data->gradient_results_sets[i];
+    const trajopt_common::GradientResultsSet& r = collision_data->gradient_results_sets[i];
     Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), position_var_->GetRows());
 
     // Collision is 1 x n_dof
diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
index bf991464..e4d0d5d5 100644
--- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
+++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp
@@ -23,15 +23,16 @@
  */
 
 #include <trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h>
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
+#include <trajopt_common/collision_utils.h>
 
 namespace trajopt_ifopt
 {
-SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(std::shared_ptr<CollisionCache> collision_cache,
-                                                                   tesseract_kinematics::JointGroup::ConstPtr manip,
-                                                                   tesseract_environment::Environment::ConstPtr env,
-                                                                   TrajOptCollisionConfig::ConstPtr collision_config,
-                                                                   bool dynamic_environment)
+SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(
+    std::shared_ptr<trajopt_common::CollisionCache> collision_cache,
+    tesseract_kinematics::JointGroup::ConstPtr manip,
+    tesseract_environment::Environment::ConstPtr env,
+    trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config,
+    bool dynamic_environment)
   : collision_cache_(std::move(collision_cache))
   , manip_(std::move(manip))
   , env_(std::move(env))
@@ -73,7 +74,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(std::shared_p
       collision_config_->collision_margin_buffer);
 }
 
-CollisionCacheData::ConstPtr
+trajopt_common::CollisionCacheData::ConstPtr
 SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
                                                  std::size_t bounds_size)
 {
@@ -86,24 +87,24 @@ SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::V
   }
 
   CONSOLE_BRIDGE_logDebug("Not using cached collision check");
-  auto data = std::make_shared<CollisionCacheData>();
+  auto data = std::make_shared<trajopt_common::CollisionCacheData>();
   CalcCollisionsHelper(dof_vals, data->contact_results_map);
 
   for (const auto& pair : data->contact_results_map)
   {
-    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, GradientResultsSet>;
+    using ShapeGrsType = std::map<std::pair<std::size_t, std::size_t>, trajopt_common::GradientResultsSet>;
     ShapeGrsType shape_grs;
     const double coeff =
         collision_config_->collision_coeff_data.getPairCollisionCoeff(pair.first.first, pair.first.second);
     for (const tesseract_collision::ContactResult& dist_result : pair.second)
     {
-      const std::size_t shape_hash0 = cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
-      const std::size_t shape_hash1 = cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
+      const std::size_t shape_hash0 = trajopt_common::cantorHash(dist_result.shape_id[0], dist_result.subshape_id[0]);
+      const std::size_t shape_hash1 = trajopt_common::cantorHash(dist_result.shape_id[1], dist_result.subshape_id[1]);
       auto shape_key = std::make_pair(shape_hash0, shape_hash1);
       auto it = shape_grs.find(shape_key);
       if (it == shape_grs.end())
       {
-        GradientResultsSet grs;
+        trajopt_common::GradientResultsSet grs;
         grs.key = pair.first;
         grs.shape_key = shape_key;
         grs.coeff = coeff;
@@ -131,7 +132,7 @@ SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref<const Eigen::V
   {
     std::sort(data->gradient_results_sets.begin(),
               data->gradient_results_sets.end(),
-              [](const GradientResultsSet& a, const GradientResultsSet& b) {
+              [](const trajopt_common::GradientResultsSet& a, const trajopt_common::GradientResultsSet& b) {
                 return a.max_error[0].error > b.max_error[0].error;
               });
   }
@@ -180,17 +181,19 @@ void SingleTimestepCollisionEvaluator::CalcCollisionsHelper(const Eigen::Ref<con
   dist_results.filter(filter);
 }
 
-GradientResults SingleTimestepCollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals,
-                                                              const tesseract_collision::ContactResult& contact_result)
+trajopt_common::GradientResults
+SingleTimestepCollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals,
+                                              const tesseract_collision::ContactResult& contact_result)
 {
   // Contains the contact distance threshold and coefficient for the given link pair
   double margin = collision_config_->contact_manager_config.margin_data.getPairCollisionMargin(
       contact_result.link_names[0], contact_result.link_names[1]);
 
-  return getGradient(dofvals, contact_result, margin, collision_config_->collision_margin_buffer, manip_);
+  return trajopt_common::getGradient(
+      dofvals, contact_result, margin, collision_config_->collision_margin_buffer, manip_);
 }
 
-const TrajOptCollisionConfig& SingleTimestepCollisionEvaluator::GetCollisionConfig() const
+const trajopt_common::TrajOptCollisionConfig& SingleTimestepCollisionEvaluator::GetCollisionConfig() const
 {
   return *collision_config_;
 }
diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp
index 29a2c56b..d821ecd5 100644
--- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp
+++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp
@@ -32,9 +32,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH
 TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h>
-#include <trajopt_ifopt/constraints/collision/collision_utils.h>
 #include <trajopt_ifopt/constraints/collision/weighted_average_methods.h>
 #include <trajopt_ifopt/utils/numeric_differentiation.h>
+#include <trajopt_common/collision_utils.h>
 
 namespace trajopt_ifopt
 {
@@ -95,7 +95,7 @@ Eigen::VectorXd
 DiscreteCollisionNumericalConstraint::CalcValues(const Eigen::Ref<const Eigen::VectorXd>& joint_vals) const
 {
   // Check the collisions
-  CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
+  auto collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
   double margin_buffer = collision_evaluator_->GetCollisionConfig().collision_margin_buffer;
   Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast<Eigen::Index>(bounds_.size()), -margin_buffer);
 
@@ -125,7 +125,7 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Ref<co
   if (!triplet_list_.empty())                                               // NOLINT
     jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end());  // NOLINT
 
-  CollisionCacheData::ConstPtr collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
+  auto collision_data = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size());
   if (collision_data->gradient_results_sets.empty())
     return;
 
@@ -138,11 +138,11 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Ref<co
   for (int j = 0; j < n_dof_; j++)
   {
     jv(j) = joint_vals(j) + delta;
-    CollisionCacheData::ConstPtr collision_data_delta = collision_evaluator_->CalcCollisions(jv, bounds_.size());
+    auto collision_data_delta = collision_evaluator_->CalcCollisions(jv, bounds_.size());
     for (int i = 0; i < static_cast<int>(cnt); ++i)
     {
-      const GradientResultsSet& baseline = collision_data->gradient_results_sets[static_cast<std::size_t>(i)];
-      auto fn = [&baseline](const GradientResultsSet& cr) {
+      const auto& baseline = collision_data->gradient_results_sets[static_cast<std::size_t>(i)];
+      auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) {
         return (cr.key == baseline.key && cr.shape_key == baseline.shape_key);
       };
 
diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp
index 019a311b..c37240a1 100644
--- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp
+++ b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp
@@ -26,7 +26,7 @@
 
 namespace trajopt_ifopt
 {
-Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_set,
+Eigen::VectorXd getWeightedAvgGradientT0(const trajopt_common::GradientResultsSet& grad_results_set,
                                          double max_error_with_buffer,
                                          Eigen::Index size)
 {
@@ -64,7 +64,7 @@ Eigen::VectorXd getWeightedAvgGradientT0(const GradientResultsSet& grad_results_
   return (1.0 / total_weight) * grad_results_set.coeff * grad_vec;
 }
 
-Eigen::VectorXd getWeightedAvgGradientT1(const GradientResultsSet& grad_results_set,
+Eigen::VectorXd getWeightedAvgGradientT1(const trajopt_common::GradientResultsSet& grad_results_set,
                                          double max_error_with_buffer,
                                          Eigen::Index size)
 {