Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Update to leverage forward declarations #395

Merged
merged 1 commit into from
Apr 11, 2024
+2,016 −1,244
Merged

Update to leverage forward declarations #395

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
100 changes: 56 additions & 44 deletions trajopt/include/trajopt/collision_terms.hpp
View file
Open in desktop
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
View file
Open in desktop

This file was deleted.

17 changes: 13 additions & 4 deletions trajopt/include/trajopt/file_write_callback.hpp
View file
Open in desktop
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
View file
Open in desktop
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
Loading