Update based on changes in trajopt

Author: Levi-Armstrong

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 };

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;

tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h

@@ -84,22 +84,22 @@ 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);
 
 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,

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