Update to leverage forward declarations

tesseract-robotics · Mar 11, 2024 · 79c6162 · 79c6162
1 parent 35bfb93
commit 79c6162
Show file tree

Hide file tree

Showing 113 changed files with 1,929 additions and 1,224 deletions.
diff --git a/trajopt/include/trajopt/collision_terms.hpp b/trajopt/include/trajopt/collision_terms.hpp
@@ -1,11 +1,19 @@
 #pragma once
-#include <tesseract_environment/environment.h>
-#include <tesseract_environment/utils.h>
-#include <tesseract_kinematics/core/joint_group.h>
+#include <vector>
+#include <memory>
+#include <Eigen/Core>
+
+#include <tesseract_collision/core/fwd.h>
+#include <tesseract_collision/core/types.h>
+#include <tesseract_kinematics/core/fwd.h>
+#include <tesseract_environment/fwd.h>
+#include <tesseract_visualization/fwd.h>
+
+#include <trajopt_common/fwd.h>
+#include <trajopt_sco/sco_common.hpp>
+
 #include <trajopt/cache.hxx>
-#include <trajopt/common.hpp>
-#include <trajopt_sco/modeling.hpp>
-#include <trajopt_common/utils.hpp>
+#include <trajopt/typedefs.hpp>
 
 namespace trajopt
 {
@@ -49,6 +57,8 @@ enum class CollisionExpressionEvaluatorType
 /** @brief A data structure to contain a links gradient results */
 struct LinkGradientResults
 {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /** @brief Indicates if gradient results are available */
   bool has_gradient{ false };
 
@@ -62,6 +72,8 @@ struct LinkGradientResults
 /** @brief A data structure to contain a link pair gradient results */
 struct GradientResults
 {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
   /**
    * @brief Construct the GradientResults
    * @param data The link pair safety margin data
@@ -90,9 +102,9 @@ struct CollisionEvaluator
   using ConstPtr = std::shared_ptr<const CollisionEvaluator>;
 
   // NOLINTNEXTLINE
-  CollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip,
-                     tesseract_environment::Environment::ConstPtr env,
-                     trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CollisionEvaluator(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                     std::shared_ptr<const tesseract_environment::Environment> env,
+                     std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                      tesseract_collision::ContactTestType contact_test_type,
                      double longest_valid_segment_length,
                      double safety_margin_buffer,
@@ -132,7 +144,7 @@ struct CollisionEvaluator
    * @param plotter Plotter
    * @param x Optimizer variables
    */
-  virtual void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) = 0;
+  virtual void Plot(const std::shared_ptr<tesseract_visualization::Visualization>& plotter, const DblVec& x) = 0;
 
   /**
    * @brief Get the specific optimizer variables associated with this evaluator.
@@ -208,19 +220,19 @@ struct CollisionEvaluator
    * @brief Get the safety margin information.
    * @return Safety margin information
    */
-  trajopt_common::SafetyMarginData::ConstPtr getSafetyMarginData() const { return safety_margin_data_; }
+  std::shared_ptr<const trajopt_common::SafetyMarginData> getSafetyMarginData() const;
 
   /** @brief The collision results cached results */
 
   Cache<std::size_t, std::pair<ContactResultMapConstPtr, ContactResultVectorConstPtr>> m_cache{ 2 };
 
 protected:
-  tesseract_kinematics::JointGroup::ConstPtr manip_;
-  tesseract_environment::Environment::ConstPtr env_;
+  std::shared_ptr<const tesseract_kinematics::JointGroup> manip_;
+  std::shared_ptr<const tesseract_environment::Environment> env_;
   std::vector<std::string> env_active_link_names_;
   std::vector<std::string> manip_active_link_names_;
   std::vector<std::string> diff_active_link_names_;
-  trajopt_common::SafetyMarginData::ConstPtr safety_margin_data_;
+  std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data_;
   double safety_margin_buffer_{ 0 };
   tesseract_collision::ContactTestType contact_test_type_{ tesseract_collision::ContactTestType::ALL };
   double longest_valid_segment_length_{ 0.05 };
@@ -364,9 +376,9 @@ struct SingleTimestepCollisionEvaluator : public CollisionEvaluator
   using Ptr = std::shared_ptr<SingleTimestepCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const SingleTimestepCollisionEvaluator>;
 
-  SingleTimestepCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip,
-                                   tesseract_environment::Environment::ConstPtr env,
-                                   trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  SingleTimestepCollisionEvaluator(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                                   std::shared_ptr<const tesseract_environment::Environment> env,
+                                   std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                                    tesseract_collision::ContactTestType contact_test_type,
                                    sco::VarVector vars,
                                    CollisionExpressionEvaluatorType type,
@@ -390,11 +402,11 @@ struct SingleTimestepCollisionEvaluator : public CollisionEvaluator
    */
   void CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals,
                       tesseract_collision::ContactResultMap& dist_results);
-  void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override;
+  void Plot(const std::shared_ptr<tesseract_visualization::Visualization>& plotter, const DblVec& x) override;
   sco::VarVector GetVars() override { return vars0_; }
 
 private:
-  tesseract_collision::DiscreteContactManager::Ptr contact_manager_;
+  std::shared_ptr<tesseract_collision::DiscreteContactManager> contact_manager_;
   std::function<void(const DblVec&, sco::AffExprVector&, std::vector<std::array<double, 2>>&)> fn_;
 };
 
@@ -408,9 +420,9 @@ struct CastCollisionEvaluator : public CollisionEvaluator
   using Ptr = std::shared_ptr<CastCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const CastCollisionEvaluator>;
 
-  CastCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip,
-                         tesseract_environment::Environment::ConstPtr env,
-                         trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CastCollisionEvaluator(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                         std::shared_ptr<const tesseract_environment::Environment> env,
+                         std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                          tesseract_collision::ContactTestType contact_test_type,
                          double longest_valid_segment_length,
                          sco::VarVector vars0,
@@ -430,11 +442,11 @@ struct CastCollisionEvaluator : public CollisionEvaluator
   void CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                       const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                       tesseract_collision::ContactResultMap& dist_results);
-  void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override;
-  sco::VarVector GetVars() override { return trajopt_common::concat(vars0_, vars1_); }
+  void Plot(const std::shared_ptr<tesseract_visualization::Visualization>& plotter, const DblVec& x) override;
+  sco::VarVector GetVars() override;
 
 private:
-  tesseract_collision::ContinuousContactManager::Ptr contact_manager_;
+  std::shared_ptr<tesseract_collision::ContinuousContactManager> contact_manager_;
   std::function<void(const DblVec&, sco::AffExprVector&, std::vector<std::array<double, 2>>&)> fn_;
 };
 
@@ -448,9 +460,9 @@ struct DiscreteCollisionEvaluator : public CollisionEvaluator
   using Ptr = std::shared_ptr<DiscreteCollisionEvaluator>;
   using ConstPtr = std::shared_ptr<const DiscreteCollisionEvaluator>;
 
-  DiscreteCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip,
-                             tesseract_environment::Environment::ConstPtr env,
-                             trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  DiscreteCollisionEvaluator(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                             std::shared_ptr<const tesseract_environment::Environment> env,
+                             std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                              tesseract_collision::ContactTestType contact_test_type,
                              double longest_valid_segment_length,
                              sco::VarVector vars0,
@@ -470,29 +482,29 @@ struct DiscreteCollisionEvaluator : public CollisionEvaluator
   void CalcCollisions(const Eigen::Ref<const Eigen::VectorXd>& dof_vals0,
                       const Eigen::Ref<const Eigen::VectorXd>& dof_vals1,
                       tesseract_collision::ContactResultMap& dist_results);
-  void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override;
-  sco::VarVector GetVars() override { return trajopt_common::concat(vars0_, vars1_); }
+  void Plot(const std::shared_ptr<tesseract_visualization::Visualization>& plotter, const DblVec& x) override;
+  sco::VarVector GetVars() override;
 
 private:
-  tesseract_collision::DiscreteContactManager::Ptr contact_manager_;
+  std::shared_ptr<tesseract_collision::DiscreteContactManager> contact_manager_;
   std::function<void(const DblVec&, sco::AffExprVector&, std::vector<std::array<double, 2>>&)> fn_;
 };
 
 class CollisionCost : public sco::Cost, public Plotter
 {
 public:
   /* constructor for single timestep */
-  CollisionCost(tesseract_kinematics::JointGroup::ConstPtr manip,
-                tesseract_environment::Environment::ConstPtr env,
-                trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CollisionCost(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                std::shared_ptr<const tesseract_environment::Environment> env,
+                std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                 tesseract_collision::ContactTestType contact_test_type,
                 sco::VarVector vars,
                 CollisionExpressionEvaluatorType type,
                 double safety_margin_buffer);
   /* constructor for discrete continuous and cast continuous cost */
-  CollisionCost(tesseract_kinematics::JointGroup::ConstPtr manip,
-                tesseract_environment::Environment::ConstPtr env,
-                trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CollisionCost(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                std::shared_ptr<const tesseract_environment::Environment> env,
+                std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                 tesseract_collision::ContactTestType contact_test_type,
                 double longest_valid_segment_length,
                 sco::VarVector vars0,
@@ -502,7 +514,7 @@ class CollisionCost : public sco::Cost, public Plotter
                 double safety_margin_buffer);
   sco::ConvexObjective::Ptr convex(const DblVec& x, sco::Model* model) override;
   double value(const DblVec&) override;
-  void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override;
+  void Plot(const std::shared_ptr<tesseract_visualization::Visualization>& plotter, const DblVec& x) override;
   sco::VarVector getVars() override { return m_calc->GetVars(); }
 
 private:
@@ -513,17 +525,17 @@ class CollisionConstraint : public sco::IneqConstraint
 {
 public:
   /* constructor for single timestep */
-  CollisionConstraint(tesseract_kinematics::JointGroup::ConstPtr manip,
-                      tesseract_environment::Environment::ConstPtr env,
-                      trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CollisionConstraint(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                      std::shared_ptr<const tesseract_environment::Environment> env,
+                      std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                       tesseract_collision::ContactTestType contact_test_type,
                       sco::VarVector vars,
                       CollisionExpressionEvaluatorType type,
                       double safety_margin_buffer);
   /* constructor for discrete continuous and cast continuous cost */
-  CollisionConstraint(tesseract_kinematics::JointGroup::ConstPtr manip,
-                      tesseract_environment::Environment::ConstPtr env,
-                      trajopt_common::SafetyMarginData::ConstPtr safety_margin_data,
+  CollisionConstraint(std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
+                      std::shared_ptr<const tesseract_environment::Environment> env,
+                      std::shared_ptr<const trajopt_common::SafetyMarginData> safety_margin_data,
                       tesseract_collision::ContactTestType contact_test_type,
                       double longest_valid_segment_length,
                       sco::VarVector vars0,

diff --git a/trajopt/include/trajopt/common.hpp b/trajopt/include/trajopt/common.hpp
diff --git a/trajopt/include/trajopt/file_write_callback.hpp b/trajopt/include/trajopt/file_write_callback.hpp
@@ -1,8 +1,17 @@
 #pragma once
-#include <trajopt/problem_description.hpp>
-#include <trajopt_sco/optimizers.hpp>
+
+#include <functional>
+#include <memory>
+
+namespace sco
+{
+class OptProb;
+struct OptResults;
+}  // namespace sco
 
 namespace trajopt
 {
-sco::Optimizer::Callback WriteCallback(std::shared_ptr<std::ofstream> file, const TrajOptProb::Ptr& prob);
-}
+class TrajOptProb;
+std::function<void(sco::OptProb*, sco::OptResults&)> WriteCallback(std::shared_ptr<std::ofstream> file,
+                                                                   const std::shared_ptr<TrajOptProb>& prob);
+}  // namespace trajopt
diff --git a/trajopt/include/trajopt/fwd.hpp b/trajopt/include/trajopt/fwd.hpp
@@ -0,0 +1,73 @@
+#ifndef TRAJOPT_FWD_HPP
+#define TRAJOPT_FWD_HPP
+
+namespace trajopt
+{
+// collision_terms.hpp
+enum class CollisionExpressionEvaluatorType;
+struct LinkGradientResults;
+struct GradientResults;
+struct CollisionEvaluator;
+
+// problem_description.hpp
+enum class TermType : char;
+class TrajOptProb;
+struct ProblemConstructionInfo;
+struct TrajOptResult;
+struct BasicInfo;
+struct InitInfo;
+struct TermInfo;
+struct UserDefinedTermInfo;
+struct DynamicCartPoseTermInfo;
+struct CartPoseTermInfo;
+struct CartVelTermInfo;
+struct JointPosTermInfo;
+struct JointVelTermInfo;
+struct JointAccTermInfo;
+struct JointJerkTermInfo;
+enum class CollisionEvaluatorType;
+struct CollisionTermInfo;
+struct TotalTimeTermInfo;
+struct AvoidSingularityTermInfo;
+
+// Kinematic_terms.hpp
+struct DynamicCartPoseErrCalculator;
+struct DynamicCartPoseJacCalculator;
+struct CartPoseErrCalculator;
+struct CartPoseJacCalculator;
+struct CartVelJacCalculator;
+struct CartVelErrCalculator;
+struct JointVelErrCalculator;
+struct JointVelJacCalculator;
+struct JointAccErrCalculator;
+struct JointAccJacCalculator;
+struct JointJerkErrCalculator;
+struct JointJerkJacCalculator;
+struct TimeCostCalculator;
+struct TimeCostJacCalculator;
+struct AvoidSingularityErrCalculator;
+struct AvoidSingularityJacCalculator;
+struct AvoidSingularitySubsetErrCalculator;
+struct AvoidSingularitySubsetJacCalculator;
+
+// trajectory_costs.hpp
+class JointPosEqCost;
+class JointPosIneqCost;
+class JointPosEqConstraint;
+class JointPosIneqConstraint;
+class JointVelEqCost;
+class JointVelIneqCost;
+class JointVelEqConstraint;
+class JointVelIneqConstraint;
+class JointAccEqCost;
+class JointAccIneqCost;
+class JointAccEqConstraint;
+class JointAccIneqConstraint;
+class JointJerkEqCost;
+class JointJerkIneqCost;
+class JointJerkEqConstraint;
+class JointJerkIneqConstraint;
+
+}  // namespace trajopt
+
+#endif  // TRAJOPT_FWD_HPP