Move TrajOptIfopt collision gradient types and utils to trajopt_commo…

…n package

trajopt_common/CMakeLists.txt

@@ -15,19 +15,29 @@ 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()
 
 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}

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)
 

trajopt_ifopt/include/trajopt_ifopt/cache.h → 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

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_types.h → trajopt_common/include/trajopt_common/collision_types.h

@@ -21,22 +21,20 @@
  * 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
 #include <Eigen/Eigen>
 #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

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/collision_utils.h → 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

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>

trajopt_ifopt/src/constraints/collision/collision_types.cpp → 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

trajopt_ifopt/src/constraints/collision/collision_utils.cpp → 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

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

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,