From 6083378ba866a59d45a72afb46a032f7d124efc3 Mon Sep 17 00:00:00 2001
From: Levi Armstrong <levi.armstrong@gmail.com>
Date: Tue, 10 Oct 2023 11:47:40 -0500
Subject: [PATCH] Update based on changes in trajopt

---
 tesseract_examples/src/online_planning_example.cpp        | 4 ++--
 .../profile/trajopt_ifopt_default_composite_profile.h     | 8 ++++----
 .../trajopt_ifopt/trajopt_ifopt_utils.h                   | 6 +++---
 .../trajopt_ifopt/src/trajopt_ifopt_utils.cpp             | 8 ++++----
 4 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/tesseract_examples/src/online_planning_example.cpp b/tesseract_examples/src/online_planning_example.cpp
index d470e08c635..f2e2efa1626 100644
--- a/tesseract_examples/src/online_planning_example.cpp
+++ b/tesseract_examples/src/online_planning_example.cpp
@@ -199,12 +199,12 @@ bool OnlinePlanningExample::setupProblem(const std::vector<Eigen::VectorXd>& ini
   // Add a collision cost for all steps
   double margin_coeff = 10;
   double margin = 0.1;
-  auto collision_config = std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>(margin, margin_coeff);
+  auto collision_config = std::make_shared<trajopt_common::TrajOptCollisionConfig>(margin, margin_coeff);
   collision_config->contact_request.type = tesseract_collision::ContactTestType::ALL;
   collision_config->type = CollisionEvaluatorType::DISCRETE;
   collision_config->collision_margin_buffer = 0.10;
 
-  auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(steps_);
+  auto collision_cache = std::make_shared<trajopt_common::CollisionCache>(steps_);
   if (use_continuous_)
   {
     std::array<bool, 2> position_vars_fixed{ true, false };
diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h
index 30f30362bb0..5d41886dce4 100644
--- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h
+++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h
@@ -48,12 +48,12 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile
   TrajOptIfoptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element);
 
   /** @brief Configuration info for collisions that are modeled as costs */
-  trajopt_ifopt::TrajOptCollisionConfig::Ptr collision_cost_config{
-    std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>()
+  trajopt_common::TrajOptCollisionConfig::Ptr collision_cost_config{
+    std::make_shared<trajopt_common::TrajOptCollisionConfig>()
   };
   /** @brief Configuration info for collisions that are modeled as constraints */
-  trajopt_ifopt::TrajOptCollisionConfig::Ptr collision_constraint_config{
-    std::make_shared<trajopt_ifopt::TrajOptCollisionConfig>()
+  trajopt_common::TrajOptCollisionConfig::Ptr collision_constraint_config{
+    std::make_shared<trajopt_common::TrajOptCollisionConfig>()
   };
   /** @brief If true, a joint velocity cost with a target of 0 will be applied for all timesteps Default: true*/
   bool smooth_velocities = true;
diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h
index 34fbd5ce455..eaccb8bfba4 100644
--- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h
+++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h
@@ -84,7 +84,7 @@ std::vector<ifopt::ConstraintSet::Ptr>
 createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                            const tesseract_environment::Environment::ConstPtr& env,
                            const tesseract_common::ManipulatorInfo& manip_info,
-                           const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                           const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                            const std::vector<int>& fixed_indices,
                            bool fixed_sparsity = true);
 
@@ -92,14 +92,14 @@ bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp,
                             const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                             const tesseract_environment::Environment::ConstPtr& env,
                             const tesseract_common::ManipulatorInfo& manip_info,
-                            const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                            const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                             const std::vector<int>& fixed_indices);
 
 bool addCollisionCost(trajopt_sqp::QPProblem& nlp,
                       const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                       const tesseract_environment::Environment::ConstPtr& env,
                       const tesseract_common::ManipulatorInfo& manip_info,
-                      const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                      const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                       const std::vector<int>& fixed_indices);
 
 ifopt::ConstraintSet::Ptr createJointVelocityConstraint(const Eigen::Ref<const Eigen::VectorXd>& target,
diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp
index 523c0038f35..1947d9ed9ab 100644
--- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp
+++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp
@@ -181,7 +181,7 @@ std::vector<ifopt::ConstraintSet::Ptr>
 createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                            const tesseract_environment::Environment::ConstPtr& env,
                            const tesseract_common::ManipulatorInfo& manip_info,
-                           const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                           const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                            const std::vector<int>& fixed_indices,
                            bool fixed_sparsity)
 {
@@ -190,7 +190,7 @@ createCollisionConstraints(const std::vector<trajopt_ifopt::JointPosition::Const
     return constraints;
 
   // Add a collision cost for all steps
-  auto collision_cache = std::make_shared<trajopt_ifopt::CollisionCache>(vars.size());
+  auto collision_cache = std::make_shared<trajopt_common::CollisionCache>(vars.size());
   std::unordered_map<std::string, tesseract_kinematics::JointGroup::ConstPtr> manipulators;
   if (config->type == tesseract_collision::CollisionEvaluatorType::DISCRETE)
   {
@@ -322,7 +322,7 @@ bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp,
                             const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                             const tesseract_environment::Environment::ConstPtr& env,
                             const tesseract_common::ManipulatorInfo& manip_info,
-                            const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                            const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                             const std::vector<int>& fixed_indices)
 {
   auto constraints = createCollisionConstraints(vars, env, manip_info, config, fixed_indices);
@@ -335,7 +335,7 @@ bool addCollisionCost(trajopt_sqp::QPProblem& nlp,
                       const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& vars,
                       const tesseract_environment::Environment::ConstPtr& env,
                       const tesseract_common::ManipulatorInfo& manip_info,
-                      const trajopt_ifopt::TrajOptCollisionConfig::ConstPtr& config,
+                      const trajopt_common::TrajOptCollisionConfig::ConstPtr& config,
                       const std::vector<int>& fixed_indices)
 {
   // Coefficients are applied within the constraint