Skip to content

Commit

Permalink
Update to leverage forward declarations
Browse files Browse the repository at this point in the history
  • Loading branch information
Levi-Armstrong committed Apr 11, 2024
1 parent 35bfb93 commit 87f25da
Show file tree
Hide file tree
Showing 117 changed files with 2,016 additions and 1,244 deletions.
100 changes: 56 additions & 44 deletions trajopt/include/trajopt/collision_terms.hpp
Original file line number Diff line number Diff line change
@@ -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
{
Expand Down Expand Up @@ -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 };

Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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 };
Expand Down Expand Up @@ -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,
Expand All @@ -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_;
};

Expand All @@ -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,
Expand All @@ -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_;
};

Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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:
Expand All @@ -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,
Expand Down
3 changes: 0 additions & 3 deletions trajopt/include/trajopt/common.hpp

This file was deleted.

17 changes: 13 additions & 4 deletions trajopt/include/trajopt/file_write_callback.hpp
Original file line number Diff line number Diff line change
@@ -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
73 changes: 73 additions & 0 deletions trajopt/include/trajopt/fwd.hpp
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 87f25da

Please sign in to comment.