From 79c6162dc51cad6980fad5f4065510202847d812 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 10 Mar 2024 11:42:58 -0500 Subject: [PATCH] Update to leverage forward declarations --- trajopt/include/trajopt/collision_terms.hpp | 100 +-- trajopt/include/trajopt/common.hpp | 3 - .../include/trajopt/file_write_callback.hpp | 17 +- trajopt/include/trajopt/fwd.hpp | 73 +++ trajopt/include/trajopt/kinematic_terms.hpp | 65 +- trajopt/include/trajopt/plot_callback.hpp | 27 +- .../include/trajopt/problem_description.hpp | 108 ++-- trajopt/include/trajopt/trajectory_costs.hpp | 3 +- trajopt/include/trajopt/typedefs.hpp | 12 +- trajopt/src/collision_terms.cpp | 94 +-- trajopt/src/file_write_callback.cpp | 10 +- trajopt/src/kinematic_terms.cpp | 21 +- trajopt/src/plot_callback.cpp | 24 +- trajopt/src/problem_description.cpp | 108 ++-- trajopt/src/trajectory_costs.cpp | 3 + .../test/cart_position_optimization_unit.cpp | 5 +- trajopt/test/cast_cost_attached_unit.cpp | 10 +- trajopt/test/cast_cost_octomap_unit.cpp | 12 +- trajopt/test/cast_cost_unit.cpp | 7 +- trajopt/test/cast_cost_world_unit.cpp | 13 +- trajopt/test/joint_costs_unit.cpp | 46 +- trajopt/test/kinematic_costs_unit.cpp | 5 +- trajopt/test/numerical_ik_unit.cpp | 5 +- trajopt/test/planning_unit.cpp | 8 +- trajopt/test/simple_collision_unit.cpp | 7 +- .../include/trajopt_common/collision_types.h | 7 +- .../include/trajopt_common/collision_utils.h | 12 +- trajopt_common/include/trajopt_common/fwd.h | 16 + .../include/trajopt_common/utils.hpp | 1 + trajopt_common/src/collision_utils.cpp | 14 +- trajopt_common/src/utils.cpp | 5 +- .../constraints/cartesian_line_constraint.h | 19 +- .../cartesian_position_constraint.h | 21 +- .../continuous_collision_constraint.h | 20 +- .../continuous_collision_evaluators.h | 85 +-- ...ontinuous_collision_numerical_constraint.h | 17 +- .../collision/discrete_collision_constraint.h | 19 +- .../collision/discrete_collision_evaluators.h | 49 +- .../discrete_collision_numerical_constraint.h | 17 +- .../collision/weighted_average_methods.h | 4 +- .../inverse_kinematics_constraint.h | 21 +- .../joint_acceleration_constraint.h | 12 +- .../constraints/joint_jerk_constraint.h | 14 +- .../constraints/joint_position_constraint.h | 12 +- .../constraints/joint_velocity_constraint.h | 12 +- trajopt_ifopt/include/trajopt_ifopt/fwd.h | 67 ++ .../include/trajopt_ifopt/trajopt_ifopt.h | 49 -- .../trajopt_ifopt/utils/trajopt_utils.h | 13 +- .../variable_sets/joint_position_variable.h | 4 +- .../constraints/cartesian_line_constraint.cpp | 10 +- .../cartesian_position_constraint.cpp | 12 +- .../continuous_collision_constraint.cpp | 22 +- .../continuous_collision_evaluators.cpp | 33 +- ...tinuous_collision_numerical_constraint.cpp | 12 +- .../discrete_collision_constraint.cpp | 20 +- .../discrete_collision_evaluators.cpp | 21 +- ...iscrete_collision_numerical_constraint.cpp | 12 +- .../collision/weighted_average_methods.cpp | 1 + .../inverse_kinematics_constraint.cpp | 9 +- .../joint_acceleration_constraint.cpp | 3 +- .../src/constraints/joint_jerk_constraint.cpp | 5 +- .../constraints/joint_position_constraint.cpp | 7 +- .../constraints/joint_velocity_constraint.cpp | 5 +- trajopt_ifopt/src/utils/trajopt_utils.cpp | 5 + .../variable_sets/joint_position_variable.cpp | 1 + .../test/cartesian_line_constraint_unit.cpp | 4 + .../cartesian_position_constraint_unit.cpp | 3 +- trajopt_ifopt/test/cast_cost_unit.cpp | 13 +- trajopt_ifopt/test/collision_unit.cpp | 8 +- .../continuous_collision_gradient_unit.cpp | 13 +- .../test/discrete_collision_gradient_unit.cpp | 9 +- .../inverse_kinematics_constraint_unit.cpp | 3 +- trajopt_ifopt/test/joint_terms_unit.cpp | 3 +- trajopt_ifopt/test/simple_collision_unit.cpp | 9 +- trajopt_optimizers/trajopt_sqp/CMakeLists.txt | 3 +- .../callbacks/cartesian_error_plotter.h | 22 +- .../trajopt_sqp/callbacks/clear_plotter.h | 12 +- .../trajopt_sqp/callbacks/collision_plotter.h | 23 +- .../callbacks/joint_state_plotter.h | 27 +- .../trajopt_sqp/callbacks/wait_for_input.h | 13 +- .../include/trajopt_sqp/eigen_types.h | 37 ++ .../include/trajopt_sqp/expressions.h | 5 +- .../trajopt_sqp/include/trajopt_sqp/fwd.h | 44 ++ .../include/trajopt_sqp/ifopt_qp_problem.h | 12 +- .../include/trajopt_sqp/osqp_eigen_solver.h | 11 +- .../include/trajopt_sqp/qp_problem.h | 18 +- .../include/trajopt_sqp/qp_solver.h | 3 +- .../include/trajopt_sqp/sqp_callback.h | 5 +- .../include/trajopt_sqp/trajopt_qp_problem.h | 105 +-- .../trajopt_sqp/trust_region_sqp_solver.h | 36 +- .../trajopt_sqp/include/trajopt_sqp/types.h | 62 +- .../src/callbacks/cartesian_error_plotter.cpp | 17 +- .../src/callbacks/clear_plotter.cpp | 6 +- .../src/callbacks/collision_plotter.cpp | 14 +- .../src/callbacks/joint_state_plotter.cpp | 21 +- .../src/callbacks/wait_for_input.cpp | 6 +- .../trajopt_sqp/src/ifopt_qp_problem.cpp | 10 +- .../trajopt_sqp/src/osqp_eigen_solver.cpp | 117 ++-- .../trajopt_sqp/src/trajopt_qp_problem.cpp | 603 ++++++++++++------ .../src/trust_region_sqp_solver.cpp | 9 +- trajopt_optimizers/trajopt_sqp/src/types.cpp | 88 +++ .../test/cart_position_optimization_unit.cpp | 19 +- .../test/cast_cost_attached_unit.cpp | 47 +- .../test/cast_cost_octomap_unit.cpp | 30 +- .../trajopt_sqp/test/cast_cost_unit.cpp | 25 +- .../trajopt_sqp/test/cast_cost_world_unit.cpp | 28 +- .../joint_acceleration_optimization_unit.cpp | 16 +- .../test/joint_jerk_optimization_unit.cpp | 16 +- .../test/joint_position_optimization_unit.cpp | 16 +- .../test/joint_velocity_optimization_unit.cpp | 16 +- .../trajopt_sqp/test/numerical_ik_unit.cpp | 21 +- .../trajopt_sqp/test/planning_unit.cpp | 25 +- .../test/simple_collision_unit.cpp | 26 +- 113 files changed, 1929 insertions(+), 1224 deletions(-) delete mode 100644 trajopt/include/trajopt/common.hpp create mode 100644 trajopt/include/trajopt/fwd.hpp create mode 100644 trajopt_common/include/trajopt_common/fwd.h create mode 100644 trajopt_ifopt/include/trajopt_ifopt/fwd.h delete mode 100644 trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h create mode 100644 trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/eigen_types.h create mode 100644 trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h create mode 100644 trajopt_optimizers/trajopt_sqp/src/types.cpp diff --git a/trajopt/include/trajopt/collision_terms.hpp b/trajopt/include/trajopt/collision_terms.hpp index 96d1607f..9a0a8570 100644 --- a/trajopt/include/trajopt/collision_terms.hpp +++ b/trajopt/include/trajopt/collision_terms.hpp @@ -1,11 +1,19 @@ #pragma once -#include -#include -#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + #include -#include -#include -#include +#include 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; // NOLINTNEXTLINE - CollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, + CollisionEvaluator(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr 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& 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 getSafetyMarginData() const; /** @brief The collision results cached results */ Cache> m_cache{ 2 }; protected: - tesseract_kinematics::JointGroup::ConstPtr manip_; - tesseract_environment::Environment::ConstPtr env_; + std::shared_ptr manip_; + std::shared_ptr env_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data_; + std::shared_ptr 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; using ConstPtr = std::shared_ptr; - SingleTimestepCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, + SingleTimestepCollisionEvaluator(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr 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& 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& plotter, const DblVec& x) override; sco::VarVector GetVars() override { return vars0_; } private: - tesseract_collision::DiscreteContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; std::function>&)> fn_; }; @@ -408,9 +420,9 @@ struct CastCollisionEvaluator : public CollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - CastCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, + CastCollisionEvaluator(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr 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& dof_vals0, const Eigen::Ref& 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& plotter, const DblVec& x) override; + sco::VarVector GetVars() override; private: - tesseract_collision::ContinuousContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; std::function>&)> fn_; }; @@ -448,9 +460,9 @@ struct DiscreteCollisionEvaluator : public CollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - DiscreteCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, + DiscreteCollisionEvaluator(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, double longest_valid_segment_length, sco::VarVector vars0, @@ -470,11 +482,11 @@ struct DiscreteCollisionEvaluator : public CollisionEvaluator void CalcCollisions(const Eigen::Ref& dof_vals0, const Eigen::Ref& 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& plotter, const DblVec& x) override; + sco::VarVector GetVars() override; private: - tesseract_collision::DiscreteContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; std::function>&)> fn_; }; @@ -482,17 +494,17 @@ 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 manip, + std::shared_ptr env, + std::shared_ptr 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 manip, + std::shared_ptr env, + std::shared_ptr 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& 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 manip, + std::shared_ptr env, + std::shared_ptr 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 manip, + std::shared_ptr env, + std::shared_ptr 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 deleted file mode 100644 index 41b82a60..00000000 --- a/trajopt/include/trajopt/common.hpp +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once -#include -#include diff --git a/trajopt/include/trajopt/file_write_callback.hpp b/trajopt/include/trajopt/file_write_callback.hpp index bc7806b7..b5ded3e4 100644 --- a/trajopt/include/trajopt/file_write_callback.hpp +++ b/trajopt/include/trajopt/file_write_callback.hpp @@ -1,8 +1,17 @@ #pragma once -#include -#include + +#include +#include + +namespace sco +{ +class OptProb; +struct OptResults; +} // namespace sco namespace trajopt { -sco::Optimizer::Callback WriteCallback(std::shared_ptr file, const TrajOptProb::Ptr& prob); -} +class TrajOptProb; +std::function WriteCallback(std::shared_ptr file, + const std::shared_ptr& prob); +} // namespace trajopt diff --git a/trajopt/include/trajopt/fwd.hpp b/trajopt/include/trajopt/fwd.hpp new file mode 100644 index 00000000..7bfb135f --- /dev/null +++ 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 diff --git a/trajopt/include/trajopt/kinematic_terms.hpp b/trajopt/include/trajopt/kinematic_terms.hpp index 7d8dae50..32206afd 100644 --- a/trajopt/include/trajopt/kinematic_terms.hpp +++ b/trajopt/include/trajopt/kinematic_terms.hpp @@ -1,18 +1,13 @@ #pragma once #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include - -#include -#include -#include -#include -#include +#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include namespace trajopt { @@ -31,7 +26,7 @@ struct DynamicCartPoseErrCalculator : public TrajOptVectorOfVector EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** @brief Manipulator kinematics object */ - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; /** @brief The name of the link to track relative to target_frame_*/ std::string source_frame_; @@ -58,7 +53,7 @@ struct DynamicCartPoseErrCalculator : public TrajOptVectorOfVector Eigen::VectorXi indices_; DynamicCartPoseErrCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(), @@ -67,7 +62,8 @@ struct DynamicCartPoseErrCalculator : public TrajOptVectorOfVector const Eigen::VectorXd& lower_tolerance = {}, const Eigen::VectorXd& upper_tolerance = {}); - void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const Eigen::VectorXd& dof_vals) override; + void Plot(const std::shared_ptr& plotter, + const Eigen::VectorXd& dof_vals) override; Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; }; @@ -78,7 +74,7 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** @brief Manipulator kinematics object */ - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; /** @brief The name of the link to track relative to target_frame_*/ std::string source_frame_; @@ -104,7 +100,7 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector double epsilon_; DynamicCartPoseJacCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(), @@ -121,7 +117,7 @@ struct DynamicCartPoseJacCalculator : sco::MatrixOfVector struct CartPoseErrCalculator : public TrajOptVectorOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; /** @brief The name of the link to track relative to target_frame_*/ std::string source_frame_; @@ -151,7 +147,7 @@ struct CartPoseErrCalculator : public TrajOptVectorOfVector Eigen::VectorXi indices_; CartPoseErrCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(), @@ -160,7 +156,8 @@ struct CartPoseErrCalculator : public TrajOptVectorOfVector const Eigen::VectorXd& lower_tolerance = {}, const Eigen::VectorXd& upper_tolerance = {}); - void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const Eigen::VectorXd& dof_vals) override; + void Plot(const std::shared_ptr& plotter, + const Eigen::VectorXd& dof_vals) override; Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; }; @@ -170,7 +167,7 @@ struct CartPoseJacCalculator : sco::MatrixOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; /** @brief The name of the link to track relative to target_frame_*/ std::string source_frame_; @@ -202,7 +199,7 @@ struct CartPoseJacCalculator : sco::MatrixOfVector double epsilon_; CartPoseJacCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(), @@ -219,11 +216,11 @@ struct CartPoseJacCalculator : sco::MatrixOfVector struct CartVelJacCalculator : sco::MatrixOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; double limit_; std::string link_; Eigen::Isometry3d tcp_; - CartVelJacCalculator(tesseract_kinematics::JointGroup::ConstPtr manip, + CartVelJacCalculator(std::shared_ptr manip, std::string link, double limit, const Eigen::Isometry3d& tcp = Eigen::Isometry3d::Identity()); @@ -238,11 +235,11 @@ struct CartVelJacCalculator : sco::MatrixOfVector struct CartVelErrCalculator : sco::VectorOfVector { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - tesseract_kinematics::JointGroup::ConstPtr manip_; + std::shared_ptr manip_; std::string link_; double limit_; Eigen::Isometry3d tcp_; - CartVelErrCalculator(tesseract_kinematics::JointGroup::ConstPtr manip, + CartVelErrCalculator(std::shared_ptr manip, std::string link, double limit, const Eigen::Isometry3d& tcp = Eigen::Isometry3d::Identity()); @@ -322,14 +319,14 @@ struct TimeCostJacCalculator : sco::MatrixOfVector struct AvoidSingularityErrCalculator : sco::VectorOfVector { /** @brief Forward kinematics (and robot jacobian) calculator */ - tesseract_kinematics::JointGroup::ConstPtr fwd_kin_; + std::shared_ptr fwd_kin_; /** @brief The name of the robot link for which to calculate the robot jacobian (required because of kinematic trees) */ std::string link_name_; /** @brief Damping factor to prevent the cost from becoming infinite when the smallest singular value is very close or * equal to zero */ double lambda_; - AvoidSingularityErrCalculator(tesseract_kinematics::JointGroup::ConstPtr fwd_kin, + AvoidSingularityErrCalculator(std::shared_ptr fwd_kin, std::string link_name, double lambda = 1.0e-3) : fwd_kin_(std::move(fwd_kin)), link_name_(std::move(link_name)), lambda_(lambda) @@ -342,7 +339,7 @@ struct AvoidSingularityErrCalculator : sco::VectorOfVector struct AvoidSingularityJacCalculator : sco::MatrixOfVector { /** @brief Forward kinematics (and robot jacobian) calculator */ - tesseract_kinematics::JointGroup::ConstPtr fwd_kin_; + std::shared_ptr fwd_kin_; /** @brief The name of the robot link for which to calculate the robot jacobian (required because of kinematic trees) */ std::string link_name_; @@ -352,7 +349,7 @@ struct AvoidSingularityJacCalculator : sco::MatrixOfVector /** @brief Small number used to perturb each joint in the current state to calculate the partial derivative of the * robot jacobian */ double eps_; - AvoidSingularityJacCalculator(tesseract_kinematics::JointGroup::ConstPtr fwd_kin, + AvoidSingularityJacCalculator(std::shared_ptr fwd_kin, std::string link_name, double lambda = 1.0e-3, double eps = 1.0e-6) @@ -372,9 +369,9 @@ struct AvoidSingularityJacCalculator : sco::MatrixOfVector struct AvoidSingularitySubsetErrCalculator : AvoidSingularityErrCalculator { /** @brief Forward kinematics (and robot jacobian) calculator for the optimization problem's full set of joints */ - tesseract_kinematics::JointGroup::ConstPtr superset_kin_; - AvoidSingularitySubsetErrCalculator(tesseract_kinematics::JointGroup::ConstPtr subset_kin, - tesseract_kinematics::JointGroup::ConstPtr superset_kin, + std::shared_ptr superset_kin_; + AvoidSingularitySubsetErrCalculator(std::shared_ptr subset_kin, + std::shared_ptr superset_kin, std::string link_name, double lambda = 1.0e-3) : AvoidSingularityErrCalculator(std::move(subset_kin), std::move(link_name), lambda) @@ -387,9 +384,9 @@ struct AvoidSingularitySubsetErrCalculator : AvoidSingularityErrCalculator /** @brief Jacobian calculator for the subset singularity avoidance error */ struct AvoidSingularitySubsetJacCalculator : AvoidSingularityJacCalculator { - tesseract_kinematics::JointGroup::ConstPtr superset_kin_; - AvoidSingularitySubsetJacCalculator(tesseract_kinematics::JointGroup::ConstPtr subset_kin, - tesseract_kinematics::JointGroup::ConstPtr superset_kin, + std::shared_ptr superset_kin_; + AvoidSingularitySubsetJacCalculator(std::shared_ptr subset_kin, + std::shared_ptr superset_kin, std::string link_name, double lambda = 1.0e-3, double eps = 1.0e-6) diff --git a/trajopt/include/trajopt/plot_callback.hpp b/trajopt/include/trajopt/plot_callback.hpp index 4a12bd8f..dbbf7e45 100644 --- a/trajopt/include/trajopt/plot_callback.hpp +++ b/trajopt/include/trajopt/plot_callback.hpp @@ -1,7 +1,16 @@ #pragma once -#include -#include -#include + +#include +#include + +#include +#include + +namespace sco +{ +class OptProb; +struct OptResults; +} // namespace sco namespace trajopt { @@ -12,16 +21,18 @@ Returns a callback function suitable for an Optimizer. This callback will plot the trajectory (with translucent copies of the robot) as well as all of the Cost and Constraint functions with plot methods */ -sco::Optimizer::Callback PlotCallback(const tesseract_visualization::Visualization::Ptr& plotter); +std::function +PlotCallback(const std::shared_ptr& plotter); /** * @brief Returns a callback suitable for an optimizer but does not require the problem * @param plotter - * @param joint_names + * @param joint_namesco::s * @return */ -sco::Optimizer::Callback PlotProbCallback(const tesseract_visualization::Visualization::Ptr& plotter, - const tesseract_scene_graph::StateSolver& state_solver, - const std::vector& joint_names); +std::function +PlotProbCallback(const std::shared_ptr& plotter, + const tesseract_scene_graph::StateSolver& state_solver, + const std::vector& joint_names); } // namespace trajopt diff --git a/trajopt/include/trajopt/problem_description.hpp b/trajopt/include/trajopt/problem_description.hpp index 35b737df..38cd481a 100644 --- a/trajopt/include/trajopt/problem_description.hpp +++ b/trajopt/include/trajopt/problem_description.hpp @@ -2,13 +2,18 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include +#include +#include + +#include #include +#include namespace sco { struct OptResults; @@ -21,13 +26,38 @@ using TrajOptResponse = Json::Value; struct ProblemConstructionInfo; -enum TermType +enum class TermType : char { + TT_INVALID = 0, // 0000 0000 TT_COST = 0x1, // 0000 0001 TT_CNT = 0x2, // 0000 0010 TT_USE_TIME = 0x4, // 0000 0100 }; +inline TermType operator|(TermType lhs, TermType rhs) +{ + using T = std::underlying_type::type; + return TermType(static_cast(lhs) | static_cast(rhs)); +} + +inline TermType operator&(TermType lhs, TermType rhs) +{ + using T = std::underlying_type::type; + return TermType(static_cast(lhs) & static_cast(rhs)); +} + +inline TermType operator^(TermType lhs, TermType rhs) +{ + using T = std::underlying_type::type; + return TermType(static_cast(lhs) ^ static_cast(rhs)); +} + +inline TermType operator~(TermType rhs) +{ + using T = std::underlying_type::type; + return TermType(~static_cast(rhs)); +} + #define DEFINE_CREATE(classname) \ static TermInfo::Ptr create() { return std::make_shared(); } @@ -57,8 +87,8 @@ class TrajOptProb : public sco::OptProb /** @brief Returns the problem DOF. This is the number of columns in the optimization matrix. * Note that this is not necessarily the same as the kinematic DOF.*/ int GetNumDOF() { return m_traj_vars.cols(); } - tesseract_kinematics::JointGroup::ConstPtr GetKin() { return m_kin; } - tesseract_environment::Environment::ConstPtr GetEnv() { return m_env; } + std::shared_ptr GetKin() { return m_kin; } + std::shared_ptr GetEnv() { return m_env; } void SetInitTraj(const TrajArray& x) { m_init_traj = x; } TrajArray GetInitTraj() { return m_init_traj; } friend TrajOptProb::Ptr ConstructProblem(const ProblemConstructionInfo&); @@ -71,8 +101,8 @@ class TrajOptProb : public sco::OptProb /** @brief If true, the last column in the optimization matrix will be 1/dt */ bool has_time; VarArray m_traj_vars; - tesseract_kinematics::JointGroup::ConstPtr m_kin; - tesseract_environment::Environment::ConstPtr m_env; + std::shared_ptr m_kin; + std::shared_ptr m_env; TrajArray m_init_traj; }; @@ -171,8 +201,8 @@ struct TermInfo using Ptr = std::shared_ptr; std::string name; - int term_type{ -1 }; - int getSupportedTypes() const { return supported_term_types_; } + TermType term_type{ TermType::TT_INVALID }; + TermType getSupportedTypes() const { return supported_term_types_; } virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; virtual void hatch(TrajOptProb& prob) = 0; @@ -192,11 +222,11 @@ struct TermInfo TermInfo& operator=(TermInfo&&) = default; protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) {} + TermInfo(TermType supported_term_types) : supported_term_types_(supported_term_types) {} private: static std::map name2maker; // NOLINT - int supported_term_types_; + TermType supported_term_types_; }; /** @@ -211,12 +241,12 @@ struct ProblemConstructionInfo std::vector cnt_infos; InitInfo init_info; - tesseract_environment::Environment::ConstPtr env; - tesseract_kinematics::JointGroup::ConstPtr kin; + std::shared_ptr env; + std::shared_ptr kin; std::vector callbacks; - ProblemConstructionInfo(tesseract_environment::Environment::ConstPtr env) : env(std::move(env)) {} + ProblemConstructionInfo(std::shared_ptr env) : env(std::move(env)) {} void fromJson(const Json::Value& v); @@ -275,7 +305,7 @@ struct UserDefinedTermInfo : public TermInfo DEFINE_CREATE(UserDefinedTermInfo) /** @brief Initialize term with it's supported types */ - UserDefinedTermInfo() : TermInfo(TT_COST | TT_CNT) {} + UserDefinedTermInfo() : TermInfo{ TermType::TT_COST | TermType::TT_CNT } {} }; /** @brief This is used when the goal frame is not fixed in space */ @@ -317,7 +347,7 @@ struct DynamicCartPoseTermInfo : public TermInfo /** @brief This term is used when the goal frame is fixed in cartesian space - Set term_type == TT_COST or TT_CNT for cost or constraint. + Set term_type == TermType::TT_COST or TermType::TT_CNT for cost or constraint. */ struct CartPoseTermInfo : public TermInfo { @@ -375,7 +405,7 @@ struct CartVelTermInfo : public TermInfo DEFINE_CREATE(CartVelTermInfo) /** @brief Initialize term with it's supported types */ - CartVelTermInfo() : TermInfo(TT_COST | TT_CNT) {} + CartVelTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {} }; /** @@ -409,7 +439,7 @@ struct JointPosTermInfo : public TermInfo DEFINE_CREATE(JointPosTermInfo) /** @brief Initialize term with it's supported types */ - JointPosTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {} + JointPosTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT | TermType::TT_USE_TIME) {} }; /** @@ -418,12 +448,12 @@ struct JointPosTermInfo : public TermInfo Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, to the joint velocity subject to the following cases. -* term_type = TT_COST +* term_type = TermType::TT_COST ** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs ** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and velocity > lower_limit, no penalty. -* term_type = TT_CNT +* term_type = TermType::TT_CNT ** upper_limit = lower_limit = 0 - Equality constraint is applied ** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < upper_limit and velocity > lower_limit @@ -459,7 +489,7 @@ struct JointVelTermInfo : public TermInfo DEFINE_CREATE(JointVelTermInfo) /** @brief Initialize term with it's supported types */ - JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {} + JointVelTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT | TermType::TT_USE_TIME) {} }; /** @@ -468,12 +498,12 @@ struct JointVelTermInfo : public TermInfo Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, to the joint velocity subject to the following cases. -* term_type = TT_COST +* term_type = TermType::TT_COST ** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs ** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If acceleration < upper_limit and acceleration > lower_limit, no penalty. -* term_type = TT_CNT +* term_type = TermType::TT_CNT ** upper_limit = lower_limit = 0 - Equality constraint is applied ** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when acceleration < upper_limit and acceleration > lower_limit @@ -504,7 +534,7 @@ struct JointAccTermInfo : public TermInfo DEFINE_CREATE(JointAccTermInfo) /** @brief Initialize term with it's supported types */ - JointAccTermInfo() : TermInfo(TT_COST | TT_CNT) {} + JointAccTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {} }; /** @@ -513,12 +543,12 @@ struct JointAccTermInfo : public TermInfo Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, to the joint velocity subject to the following cases. -* term_type = TT_COST +* term_type = TermType::TT_COST ** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs ** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If jerk < upper_limit and jerk > lower_limit, no penalty. -* term_type = TT_CNT +* term_type = TermType::TT_CNT ** upper_limit = lower_limit = 0 - Equality constraint is applied ** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when jerk < upper_limit and jerk > lower_limit @@ -549,7 +579,7 @@ struct JointJerkTermInfo : public TermInfo DEFINE_CREATE(JointJerkTermInfo) /** @brief Initialize term with it's supported types */ - JointJerkTermInfo() : TermInfo(TT_COST | TT_CNT) {} + JointJerkTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {} }; enum class CollisionEvaluatorType @@ -615,7 +645,7 @@ struct CollisionTermInfo : public TermInfo void hatch(TrajOptProb& prob) override; DEFINE_CREATE(CollisionTermInfo) - CollisionTermInfo() : TermInfo(TT_COST | TT_CNT) {} + CollisionTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) {} }; /** @@ -632,21 +662,16 @@ struct TotalTimeTermInfo : public TermInfo void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; DEFINE_CREATE(TotalTimeTermInfo) - TotalTimeTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) {} + TotalTimeTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT | TermType::TT_USE_TIME) {} }; -TrajOptProb::Ptr ConstructProblem(const ProblemConstructionInfo&); -TrajOptProb::Ptr ConstructProblem(const Json::Value&, const tesseract_environment::Environment::ConstPtr& env); -TrajOptResult::Ptr OptimizeProblem(const TrajOptProb::Ptr&, - const tesseract_visualization::Visualization::Ptr& plotter = nullptr); - /** * @brief Applies a cost to avoid kinematic singularities */ struct AvoidSingularityTermInfo : public TermInfo { /** @brief The forward kinematics solver used to calculate the jacobian for which to do singularity avoidance */ - tesseract_kinematics::JointGroup::ConstPtr subset_kin_; + std::shared_ptr subset_kin_; /** @brief Damping factor used to prevent numerical instability in the singularity avoidance cost as the smallest * singular value approaches zero */ double lambda{ 0.1 }; @@ -660,10 +685,17 @@ struct AvoidSingularityTermInfo : public TermInfo void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; DEFINE_CREATE(AvoidSingularityTermInfo) - AvoidSingularityTermInfo(tesseract_kinematics::JointGroup::ConstPtr subset_kin = nullptr, double lambda_ = 0.1) - : TermInfo(TT_COST | TT_CNT), subset_kin_(std::move(subset_kin)), lambda(lambda_) + AvoidSingularityTermInfo(std::shared_ptr subset_kin = nullptr, + double lambda_ = 0.1) + : TermInfo(TermType::TT_COST | TermType::TT_CNT), subset_kin_(std::move(subset_kin)), lambda(lambda_) { } }; +TrajOptProb::Ptr ConstructProblem(const ProblemConstructionInfo&); +TrajOptProb::Ptr ConstructProblem(const Json::Value&, + const std::shared_ptr& env); +TrajOptResult::Ptr OptimizeProblem(const TrajOptProb::Ptr&, + const std::shared_ptr& plotter = nullptr); + } // namespace trajopt diff --git a/trajopt/include/trajopt/trajectory_costs.hpp b/trajopt/include/trajopt/trajectory_costs.hpp index 091ee8fd..dd5eb667 100644 --- a/trajopt/include/trajopt/trajectory_costs.hpp +++ b/trajopt/include/trajopt/trajectory_costs.hpp @@ -4,9 +4,8 @@ Simple quadratic costs on trajectory */ -#include +#include #include -#include namespace trajopt { diff --git a/trajopt/include/trajopt/typedefs.hpp b/trajopt/include/trajopt/typedefs.hpp index 2aca8e68..4091a0be 100644 --- a/trajopt/include/trajopt/typedefs.hpp +++ b/trajopt/include/trajopt/typedefs.hpp @@ -4,8 +4,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include - -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -45,14 +44,15 @@ class Plotter Plotter(Plotter&&) = default; Plotter& operator=(Plotter&&) = default; - virtual void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) = 0; + virtual void Plot(const std::shared_ptr& plotter, const DblVec& x) = 0; }; /** @brief Adds plotting to the VectorOfVector class in trajopt_sco */ class TrajOptVectorOfVector : public sco::VectorOfVector { public: - virtual void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const Eigen::VectorXd& dof_vals) = 0; + virtual void Plot(const std::shared_ptr& plotter, + const Eigen::VectorXd& dof_vals) = 0; }; /** @brief Adds plotting to the CostFromErrFunc class in trajopt_sco */ @@ -80,7 +80,7 @@ class TrajOptCostFromErrFunc : public sco::CostFromErrFunc, public Plotter { } - void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override + void Plot(const std::shared_ptr& plotter, const DblVec& x) override { // If error function has a inherited from TrajOptVectorOfVector, call its Plot function if (auto* plt = dynamic_cast(f_.get())) @@ -116,7 +116,7 @@ class TrajOptConstraintFromErrFunc : public sco::ConstraintFromErrFunc, public P { } - void Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) override + void Plot(const std::shared_ptr& plotter, const DblVec& x) override { // If error function has a inherited from TrajOptVectorOfVector, call its Plot function if (auto* plt = dynamic_cast(f_.get())) diff --git a/trajopt/src/collision_terms.cpp b/trajopt/src/collision_terms.cpp index faaffa35..0595f35e 100644 --- a/trajopt/src/collision_terms.cpp +++ b/trajopt/src/collision_terms.cpp @@ -1,8 +1,13 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include +#include #include #include +#include +#include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP @@ -16,6 +21,7 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include +#include namespace trajopt { @@ -319,6 +325,11 @@ GradientResults CollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals0, return GetGradient(dofvals0, dofvals1, contact_result, data, isTimestep1); } +std::shared_ptr CollisionEvaluator::getSafetyMarginData() const +{ + return safety_margin_data_; +} + void CollisionEvaluator::CollisionsToDistanceExpressions(sco::AffExprVector& exprs, std::vector>& exprs_data, const ContactResultVectorWrapper& dist_results, @@ -863,9 +874,9 @@ void CollisionEvaluator::removeInvalidContactResults(tesseract_collision::Contac } SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, sco::VarVector vars, CollisionExpressionEvaluatorType type, @@ -964,7 +975,8 @@ void SingleTimestepCollisionEvaluator::CalcDistExpressions(const DblVec& x, fn_(x, exprs, exprs_data); } -void SingleTimestepCollisionEvaluator::Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) +void SingleTimestepCollisionEvaluator::Plot(const std::shared_ptr& plotter, + const DblVec& x) { ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; @@ -1008,15 +1020,16 @@ void SingleTimestepCollisionEvaluator::Plot(const tesseract_visualization::Visua //////////////////////////////////////// -DiscreteCollisionEvaluator::DiscreteCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, - tesseract_collision::ContactTestType contact_test_type, - double longest_valid_segment_length, - sco::VarVector vars0, - sco::VarVector vars1, - CollisionExpressionEvaluatorType type, - double safety_margin_buffer) +DiscreteCollisionEvaluator::DiscreteCollisionEvaluator( + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, + tesseract_collision::ContactTestType contact_test_type, + double longest_valid_segment_length, + sco::VarVector vars0, + sco::VarVector vars1, + CollisionExpressionEvaluatorType type, + double safety_margin_buffer) : CollisionEvaluator(std::move(manip), std::move(env), std::move(safety_margin_data), @@ -1179,7 +1192,8 @@ void DiscreteCollisionEvaluator::CalcDistExpressions(const DblVec& x, fn_(x, exprs, exprs_data); } -void DiscreteCollisionEvaluator::Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) +void DiscreteCollisionEvaluator::Plot(const std::shared_ptr& plotter, + const DblVec& x) { ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; @@ -1250,17 +1264,20 @@ void DiscreteCollisionEvaluator::Plot(const tesseract_visualization::Visualizati plotter->plotMarker(cm); } +sco::VarVector DiscreteCollisionEvaluator::GetVars() { return trajopt_common::concat(vars0_, vars1_); } + //////////////////////////////////////// -CastCollisionEvaluator::CastCollisionEvaluator(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, - tesseract_collision::ContactTestType contact_test_type, - double longest_valid_segment_length, - sco::VarVector vars0, - sco::VarVector vars1, - CollisionExpressionEvaluatorType type, - double safety_margin_buffer) +CastCollisionEvaluator::CastCollisionEvaluator( + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, + tesseract_collision::ContactTestType contact_test_type, + double longest_valid_segment_length, + sco::VarVector vars0, + sco::VarVector vars1, + CollisionExpressionEvaluatorType type, + double safety_margin_buffer) : CollisionEvaluator(std::move(manip), std::move(env), std::move(safety_margin_data), @@ -1434,7 +1451,8 @@ void CastCollisionEvaluator::CalcDistExpressions(const DblVec& x, fn_(x, exprs, exprs_data); } -void CastCollisionEvaluator::Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) +void CastCollisionEvaluator::Plot(const std::shared_ptr& plotter, + const DblVec& x) { ContactResultVectorConstPtr dist_vec = GetContactResultVectorCached(x); const ContactResultVectorWrapper& dist_results = *dist_vec; @@ -1498,11 +1516,13 @@ void CastCollisionEvaluator::Plot(const tesseract_visualization::Visualization:: plotter->plotMarker(cm); } +sco::VarVector CastCollisionEvaluator::GetVars() { return trajopt_common::concat(vars0_, vars1_); } + ////////////////////////////////////////// -CollisionCost::CollisionCost(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, +CollisionCost::CollisionCost(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, sco::VarVector vars, CollisionExpressionEvaluatorType type, @@ -1518,9 +1538,9 @@ CollisionCost::CollisionCost(tesseract_kinematics::JointGroup::ConstPtr manip, safety_margin_buffer); } -CollisionCost::CollisionCost(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, +CollisionCost::CollisionCost(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, double longest_valid_segment_length, sco::VarVector vars0, @@ -1601,15 +1621,15 @@ double CollisionCost::value(const sco::DblVec& x) return out; } -void CollisionCost::Plot(const tesseract_visualization::Visualization::Ptr& plotter, const DblVec& x) +void CollisionCost::Plot(const std::shared_ptr& plotter, const DblVec& x) { m_calc->Plot(plotter, x); } // ALMOST EXACTLY COPIED FROM CollisionCost -CollisionConstraint::CollisionConstraint(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, +CollisionConstraint::CollisionConstraint(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, sco::VarVector vars, CollisionExpressionEvaluatorType type, @@ -1625,9 +1645,9 @@ CollisionConstraint::CollisionConstraint(tesseract_kinematics::JointGroup::Const safety_margin_buffer); } -CollisionConstraint::CollisionConstraint(tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::SafetyMarginData::ConstPtr safety_margin_data, +CollisionConstraint::CollisionConstraint(std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr safety_margin_data, tesseract_collision::ContactTestType contact_test_type, double longest_valid_segment_length, sco::VarVector vars0, diff --git a/trajopt/src/file_write_callback.cpp b/trajopt/src/file_write_callback.cpp index 13077477..06d1098a 100644 --- a/trajopt/src/file_write_callback.cpp +++ b/trajopt/src/file_write_callback.cpp @@ -18,10 +18,15 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include +#include + +#include #include namespace trajopt @@ -88,7 +93,8 @@ void WriteFile(const std::shared_ptr& file, *file << std::endl; } // namespace trajopt -sco::Optimizer::Callback WriteCallback(std::shared_ptr file, const TrajOptProb::Ptr& prob) +std::function WriteCallback(std::shared_ptr file, + const std::shared_ptr& prob) { if (!file->good()) { diff --git a/trajopt/src/kinematic_terms.cpp b/trajopt/src/kinematic_terms.cpp index 6775d761..9b1fe7fb 100644 --- a/trajopt/src/kinematic_terms.cpp +++ b/trajopt/src/kinematic_terms.cpp @@ -4,7 +4,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include +#include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP @@ -68,7 +72,7 @@ Eigen::VectorXd applyTolerances(const Eigen::VectorXd& error, } DynamicCartPoseErrCalculator::DynamicCartPoseErrCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset, // NOLINT(modernize-pass-by-value) @@ -129,7 +133,7 @@ VectorXd DynamicCartPoseErrCalculator::operator()(const VectorXd& dof_vals) cons return reduced_err; // This is available in 3.4 err(indices_, Eigen::all); } -void DynamicCartPoseErrCalculator::Plot(const tesseract_visualization::Visualization::Ptr& plotter, +void DynamicCartPoseErrCalculator::Plot(const std::shared_ptr& plotter, const VectorXd& dof_vals) { tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); @@ -151,7 +155,7 @@ void DynamicCartPoseErrCalculator::Plot(const tesseract_visualization::Visualiza } DynamicCartPoseJacCalculator::DynamicCartPoseJacCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset, // NOLINT(modernize-pass-by-value) @@ -198,7 +202,7 @@ MatrixXd DynamicCartPoseJacCalculator::operator()(const VectorXd& dof_vals) cons } CartPoseErrCalculator::CartPoseErrCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset, // NOLINT(modernize-pass-by-value) @@ -285,7 +289,8 @@ VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const return reduced_err; // This is available in 3.4 err(indices_, Eigen::all); } -void CartPoseErrCalculator::Plot(const tesseract_visualization::Visualization::Ptr& plotter, const VectorXd& dof_vals) +void CartPoseErrCalculator::Plot(const std::shared_ptr& plotter, + const VectorXd& dof_vals) { tesseract_common::TransformMap state = manip_->calcFwdKin(dof_vals); Eigen::Isometry3d source_tf = state[source_frame_] * source_frame_offset_; @@ -306,7 +311,7 @@ void CartPoseErrCalculator::Plot(const tesseract_visualization::Visualization::P } CartPoseJacCalculator::CartPoseJacCalculator( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset, // NOLINT(modernize-pass-by-value) @@ -376,7 +381,7 @@ MatrixXd CartPoseJacCalculator::operator()(const VectorXd& dof_vals) const return jac0; // This is available in 3.4 jac0(indices_, Eigen::all); } -CartVelJacCalculator::CartVelJacCalculator(tesseract_kinematics::JointGroup::ConstPtr manip, +CartVelJacCalculator::CartVelJacCalculator(std::shared_ptr manip, std::string link, double limit, const Eigen::Isometry3d& tcp) // NOLINT(modernize-pass-by-value) @@ -411,7 +416,7 @@ MatrixXd CartVelJacCalculator::operator()(const VectorXd& dof_vals) const return out; } -CartVelErrCalculator::CartVelErrCalculator(tesseract_kinematics::JointGroup::ConstPtr manip, +CartVelErrCalculator::CartVelErrCalculator(std::shared_ptr manip, std::string link, double limit, const Eigen::Isometry3d& tcp) // NOLINT(modernize-pass-by-value) diff --git a/trajopt/src/plot_callback.cpp b/trajopt/src/plot_callback.cpp index 9b08a4f4..44a16c89 100644 --- a/trajopt/src/plot_callback.cpp +++ b/trajopt/src/plot_callback.cpp @@ -1,19 +1,23 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include -#include +#include +#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include +#include +#include + #include -#include +#include +#include namespace trajopt { -void PlotCosts(const tesseract_visualization::Visualization::Ptr& plotter, +void PlotCosts(const std::shared_ptr& plotter, const tesseract_scene_graph::StateSolver& state_solver, const std::vector& joint_names, const std::vector& costs, @@ -48,7 +52,7 @@ void PlotCosts(const tesseract_visualization::Visualization::Ptr& plotter, plotter->waitForInput(); } -sco::Optimizer::Callback PlotCallback(const tesseract_visualization::Visualization::Ptr& plotter) +sco::Optimizer::Callback PlotCallback(const std::shared_ptr& plotter) { return [plotter](sco::OptProb* prob, sco::OptResults& results) { auto& trajopt_prob = dynamic_cast(*prob); @@ -63,7 +67,7 @@ sco::Optimizer::Callback PlotCallback(const tesseract_visualization::Visualizati }; } -void PlotProb(const tesseract_visualization::Visualization::Ptr& plotter, +void PlotProb(const std::shared_ptr& plotter, const tesseract_scene_graph::StateSolver& state_solver, const std::vector& joint_names, sco::OptProb* prob, @@ -102,7 +106,7 @@ void PlotProb(const tesseract_visualization::Visualization::Ptr& plotter, plotter->waitForInput(); } -sco::Optimizer::Callback PlotProbCallback(const tesseract_visualization::Visualization::Ptr& plotter, +sco::Optimizer::Callback PlotProbCallback(const std::shared_ptr& plotter, const tesseract_scene_graph::StateSolver& state_solver, const std::vector& joint_names) { diff --git a/trajopt/src/problem_description.cpp b/trajopt/src/problem_description.cpp index f4187793..c1f7ae19 100644 --- a/trajopt/src/problem_description.cpp +++ b/trajopt/src/problem_description.cpp @@ -2,15 +2,17 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include #include #include #include #include +#include + #include #include #include @@ -18,7 +20,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include +#include +#include +#include namespace { @@ -172,11 +176,11 @@ void ProblemConstructionInfo::readCosts(const Json::Value& v) if (use_time) { - term->term_type = TT_COST | TT_USE_TIME; + term->term_type = (TermType::TT_COST | TermType::TT_USE_TIME); basic_info.use_time = true; } else - term->term_type = TT_COST; + term->term_type = (TermType::TT_COST); term->fromJson(*this, it); json_marshal::childFromJson(it, term->name, "name", type); @@ -202,11 +206,11 @@ void ProblemConstructionInfo::readConstraints(const Json::Value& v) if (use_time) { - term->term_type = (TT_CNT | TT_USE_TIME); + term->term_type = (TermType::TT_CNT | TermType::TT_USE_TIME); basic_info.use_time = true; } else - term->term_type = (TT_CNT); + term->term_type = (TermType::TT_CNT); term->fromJson(*this, it); json_marshal::childFromJson(it, term->name, "name", type); @@ -411,27 +415,29 @@ TrajOptProb::Ptr ConstructProblem(const ProblemConstructionInfo& pci) // Check that all costs and constraints support the types that are specified in pci for (const TermInfo::Ptr& cost : pci.cost_infos) { - if (cost->term_type & TT_CNT) - CONSOLE_BRIDGE_logWarn("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str()); - if (!(cost->getSupportedTypes() & TT_COST)) + if (static_cast(cost->term_type & TermType::TT_CNT)) + CONSOLE_BRIDGE_logWarn("%s is listed as a type TermType::TT_CNT but was added to cost_infos", + (cost->name).c_str()); + if (!static_cast(cost->getSupportedTypes() & TermType::TT_COST)) PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name); - if (cost->term_type & TT_USE_TIME) + if (static_cast(cost->term_type & TermType::TT_USE_TIME)) { use_time = true; - if (!(cost->getSupportedTypes() & TT_USE_TIME)) + if (!static_cast(cost->getSupportedTypes() & TermType::TT_USE_TIME)) PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cost->name); } } for (const TermInfo::Ptr& cnt : pci.cnt_infos) { - if (cnt->term_type & TT_COST) - CONSOLE_BRIDGE_logWarn("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str()); - if (!(cnt->getSupportedTypes() & TT_CNT)) + if (static_cast(cnt->term_type & TermType::TT_COST)) + CONSOLE_BRIDGE_logWarn("%s is listed as a type TermType::TT_COST but was added to cnt_infos", + (cnt->name).c_str()); + if (!static_cast(cnt->getSupportedTypes() & TermType::TT_CNT)) PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name); - if (cnt->term_type & TT_USE_TIME) + if (static_cast(cnt->term_type & TermType::TT_USE_TIME)) { use_time = true; - if (!(cnt->getSupportedTypes() & TT_USE_TIME)) + if (!static_cast(cnt->getSupportedTypes() & TermType::TT_USE_TIME)) PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cnt->name); } } @@ -594,7 +600,7 @@ void UserDefinedTermInfo::hatch(TrajOptProb& prob) int n_dof = static_cast(prob.GetKin()->numJoints()); // Apply error calculator as either cost or constraint - if (term_type & TT_COST) + if (static_cast(term_type & TermType::TT_COST)) { for (int s = first_step; s <= last_step; ++s) { @@ -632,7 +638,7 @@ void UserDefinedTermInfo::hatch(TrajOptProb& prob) } } } - else if (term_type & TT_CNT) + else if (static_cast(term_type & TermType::TT_CNT)) { for (int s = first_step; s <= last_step; ++s) { @@ -667,7 +673,7 @@ void UserDefinedTermInfo::hatch(TrajOptProb& prob) } } -DynamicCartPoseTermInfo::DynamicCartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) +DynamicCartPoseTermInfo::DynamicCartPoseTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) { pos_coeffs = Eigen::Vector3d::Ones(); rot_coeffs = Eigen::Vector3d::Ones(); @@ -771,7 +777,7 @@ void DynamicCartPoseTermInfo::hatch(TrajOptProb& prob) Eigen::VectorXi indices = Eigen::Map(ic.data(), static_cast(ic.size())); Eigen::VectorXd coeff = Eigen::Map(c.data(), static_cast(c.size())); - if (term_type & TT_USE_TIME) + if (static_cast(term_type & TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } @@ -790,12 +796,12 @@ void DynamicCartPoseTermInfo::hatch(TrajOptProb& prob) prob.GetKin(), source_frame, target_frame, source_frame_offset, target_frame_offset, indices); // Apply error calculator as either cost or constraint - if (term_type & TT_COST) + if (static_cast(term_type & TermType::TT_COST)) { prob.addCost( std::make_shared(f, dfdx, prob.GetVarRow(timestep, 0, n_dof), coeff, sco::ABS, name)); } - else if (term_type & TT_CNT) + else if (static_cast(term_type & TermType::TT_CNT)) { prob.addConstraint(std::make_shared( f, dfdx, prob.GetVarRow(timestep, 0, n_dof), coeff, sco::EQ, name)); @@ -808,7 +814,7 @@ void DynamicCartPoseTermInfo::hatch(TrajOptProb& prob) } } -CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) +CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TermType::TT_COST | TermType::TT_CNT) { pos_coeffs = Eigen::Vector3d::Ones(); rot_coeffs = Eigen::Vector3d::Ones(); @@ -915,15 +921,15 @@ void CartPoseTermInfo::hatch(TrajOptProb& prob) Eigen::VectorXi indices = Eigen::Map(ic.data(), static_cast(ic.size())); Eigen::VectorXd coeff = Eigen::Map(c.data(), static_cast(c.size())); - if (term_type == (TT_COST | TT_USE_TIME)) + if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if (term_type == (TT_CNT | TT_USE_TIME)) + else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_COST) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { auto f = std::make_shared(prob.GetKin(), source_frame, @@ -939,7 +945,7 @@ void CartPoseTermInfo::hatch(TrajOptProb& prob) prob.addCost( std::make_shared(f, dfdx, prob.GetVarRow(timestep, 0, n_dof), coeff, sco::ABS, name)); } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_CNT) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { auto f = std::make_shared(prob.GetKin(), source_frame, @@ -987,15 +993,15 @@ void CartVelTermInfo::hatch(TrajOptProb& prob) { int n_dof = static_cast(prob.GetKin()->numJoints()); - if (term_type == (TT_COST | TT_USE_TIME)) + if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if (term_type == (TT_CNT | TT_USE_TIME)) + else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_COST) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { for (int iStep = first_step; iStep <= last_step; ++iStep) { @@ -1010,7 +1016,7 @@ void CartVelTermInfo::hatch(TrajOptProb& prob) name)); } } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_CNT) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { for (int iStep = first_step; iStep <= last_step; ++iStep) { @@ -1097,9 +1103,9 @@ void JointPosTermInfo::hatch(TrajOptProb& prob) trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); if (prob.GetHasTime()) - CONSOLE_BRIDGE_logInform("JointPosTermInfo does not differ based on setting of TT_USE_TIME"); + CONSOLE_BRIDGE_logInform("JointPosTermInfo does not differ based on setting of TermType::TT_USE_TIME"); - if (term_type & TT_COST) + if (static_cast(term_type & TermType::TT_COST)) { // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) @@ -1121,7 +1127,7 @@ void JointPosTermInfo::hatch(TrajOptProb& prob) prob.getCosts().back()->setName(name); } } - else if (term_type & TT_CNT) + else if (static_cast(term_type & TermType::TT_CNT)) { // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) @@ -1216,7 +1222,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - if (term_type == (TT_COST | TT_USE_TIME)) + if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { auto num_vels = static_cast(last_step - first_step); @@ -1255,7 +1261,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) } } } - else if (term_type == (TT_CNT | TT_USE_TIME)) + else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME)) { auto num_vels = static_cast(last_step - first_step); @@ -1296,7 +1302,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) } } } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_COST) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) @@ -1317,7 +1323,7 @@ void JointVelTermInfo::hatch(TrajOptProb& prob) prob.getCosts().back()->setName(name); } } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_CNT) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) @@ -1409,15 +1415,15 @@ void JointAccTermInfo::hatch(TrajOptProb& prob) trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - if (term_type == (TT_COST | TT_USE_TIME)) + if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if (term_type == (TT_CNT | TT_USE_TIME)) + else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_COST) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) @@ -1438,7 +1444,7 @@ void JointAccTermInfo::hatch(TrajOptProb& prob) prob.getCosts().back()->setName(name); } } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_CNT) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) @@ -1531,15 +1537,15 @@ void JointJerkTermInfo::hatch(TrajOptProb& prob) trajopt::VarArray vars = prob.GetVars(); trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - if (term_type == (TT_COST | TT_USE_TIME)) + if (term_type == (TermType::TT_COST | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if (term_type == (TT_CNT | TT_USE_TIME)) + else if (term_type == (TermType::TT_CNT | TermType::TT_USE_TIME)) { CONSOLE_BRIDGE_logError("Use time version of this term has not been defined."); } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_COST) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost if (is_upper_zeros && is_lower_zeros) @@ -1560,7 +1566,7 @@ void JointJerkTermInfo::hatch(TrajOptProb& prob) prob.getCosts().back()->setName(name); } } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) + else if (static_cast(term_type & TermType::TT_CNT) && static_cast(~(term_type | ~TermType::TT_USE_TIME))) { // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint if (is_upper_zeros && is_lower_zeros) @@ -1722,7 +1728,7 @@ void CollisionTermInfo::hatch(TrajOptProb& prob) { int n_dof = static_cast(prob.GetKin()->numJoints()); - if (term_type == TT_COST) + if (term_type == TermType::TT_COST) { if (evaluator_type != CollisionEvaluatorType::SINGLE_TIMESTEP) { @@ -1908,11 +1914,11 @@ void TotalTimeTermInfo::hatch(TrajOptProb& prob) auto f = std::make_shared(limit); auto dfdx = std::make_shared(); - if (term_type & TT_COST) + if (static_cast(term_type & TermType::TT_COST)) { prob.addCost(std::make_shared(f, dfdx, time_vars, coeff_vec, penalty_type, name)); } - else if (term_type & TT_CNT) + else if (static_cast(term_type & TermType::TT_CNT)) { prob.addConstraint( std::make_shared(f, dfdx, time_vars, coeff_vec, constraint_type, name)); @@ -1954,12 +1960,12 @@ void AvoidSingularityTermInfo::hatch(TrajOptProb& prob) for (int i = first_step; i <= last_step; ++i) { const std::string idx_name = name + "_" + std::to_string(i); - if (term_type & TT_COST) + if (static_cast(term_type & TermType::TT_COST)) { prob.addCost(std::make_shared( f, dfdx, prob.GetVarRow(i, 0, n_dof), trajopt_common::toVectorXd(coeffs), sco::ABS, idx_name)); } - else if (term_type & TT_CNT) + else if (static_cast(term_type & TermType::TT_CNT)) { prob.addConstraint(std::make_shared( f, dfdx, prob.GetVarRow(i, 0, n_dof), trajopt_common::toVectorXd(coeffs), sco::INEQ, idx_name)); diff --git a/trajopt/src/trajectory_costs.cpp b/trajopt/src/trajectory_costs.cpp index eec216ed..dd952c81 100644 --- a/trajopt/src/trajectory_costs.cpp +++ b/trajopt/src/trajectory_costs.cpp @@ -4,8 +4,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include +#include + #include #include + #include namespace diff --git a/trajopt/test/cart_position_optimization_unit.cpp b/trajopt/test/cart_position_optimization_unit.cpp index 0ebebfb6..d6e873d0 100644 --- a/trajopt/test/cart_position_optimization_unit.cpp +++ b/trajopt/test/cart_position_optimization_unit.cpp @@ -29,6 +29,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include #include #include @@ -38,6 +40,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include #include #include @@ -104,7 +107,7 @@ TEST(CartPositionOptimizationTrajoptSCO, cart_position_optimization_trajopt_sco) { auto pose = std::make_shared(); - pose->term_type = TT_CNT; + pose->term_type = TermType::TT_CNT; pose->name = "waypoint_cart_0"; pose->timestep = 0; pose->source_frame = "r_gripper_tool_frame"; diff --git a/trajopt/test/cast_cost_attached_unit.cpp b/trajopt/test/cast_cost_attached_unit.cpp index 17b674c4..5c2f8f0b 100644 --- a/trajopt/test/cast_cost_attached_unit.cpp +++ b/trajopt/test/cast_cost_attached_unit.cpp @@ -3,16 +3,24 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include +#include +#include #include #include #include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include +#include #include #include #include diff --git a/trajopt/test/cast_cost_octomap_unit.cpp b/trajopt/test/cast_cost_octomap_unit.cpp index 36a8ac1f..3e314fa1 100644 --- a/trajopt/test/cast_cost_octomap_unit.cpp +++ b/trajopt/test/cast_cost_octomap_unit.cpp @@ -5,16 +5,24 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include +#include +#include #include #include #include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include +#include #include #include #include @@ -70,7 +78,7 @@ class CastOctomapTest : public testing::TestWithParam octree->insertPointCloud(point_cloud, octomap::point3d(0, 0, 0)); // Next add objects that can be attached/detached to the scene - Octree::Ptr coll_octree = std::make_shared(octree, Octree::SubType::BOX); + Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); auto vis_box = std::make_shared(1.0, 1.0, 1.0); auto visual = std::make_shared(); diff --git a/trajopt/test/cast_cost_unit.cpp b/trajopt/test/cast_cost_unit.cpp index 43c2fbb3..5b853a96 100644 --- a/trajopt/test/cast_cost_unit.cpp +++ b/trajopt/test/cast_cost_unit.cpp @@ -2,14 +2,19 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include #include #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include +#include #include #include #include diff --git a/trajopt/test/cast_cost_world_unit.cpp b/trajopt/test/cast_cost_world_unit.cpp index 1699eb93..de8601ec 100644 --- a/trajopt/test/cast_cost_world_unit.cpp +++ b/trajopt/test/cast_cost_world_unit.cpp @@ -3,14 +3,23 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include +#include +#include +#include #include #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include +#include #include #include #include diff --git a/trajopt/test/joint_costs_unit.cpp b/trajopt/test/joint_costs_unit.cpp index 2f76b4b0..144e66e3 100644 --- a/trajopt/test/joint_costs_unit.cpp +++ b/trajopt/test/joint_costs_unit.cpp @@ -3,12 +3,16 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include +#include #include #include #include @@ -87,7 +91,7 @@ TEST_F(CostsTest, equality_jointPos) // NOLINT jv->first_step = 0; jv->last_step = 0; jv->name = "joint_pos_single"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // All the rest of the joint velocities have a cost to some non zero value @@ -97,7 +101,7 @@ TEST_F(CostsTest, equality_jointPos) // NOLINT jv2->first_step = 0; jv2->last_step = pci.basic_info.n_steps - 1; jv2->name = "joint_pos_all"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -181,7 +185,7 @@ TEST_F(CostsTest, inequality_jointPos) // NOLINT jv->first_step = 0; jv->last_step = pci.basic_info.n_steps - 1; jv->name = "joint_pos_limits"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // Joint Velocities also have a cost to some non zero value @@ -193,7 +197,7 @@ TEST_F(CostsTest, inequality_jointPos) // NOLINT jv2->first_step = 0; jv2->last_step = (pci.basic_info.n_steps - 1) / 2; jv2->name = "joint_pos_targ_1"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); auto jv3 = std::make_shared(); @@ -204,7 +208,7 @@ TEST_F(CostsTest, inequality_jointPos) // NOLINT jv3->first_step = (pci.basic_info.n_steps - 1) / 2 + 1; jv3->last_step = pci.basic_info.n_steps - 1; jv3->name = "joint_pos_targ_2"; - jv3->term_type = TT_COST; + jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -289,7 +293,7 @@ TEST_F(CostsTest, equality_jointVel) // NOLINT jv->first_step = 0; jv->last_step = 0; jv->name = "joint_vel_single"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // All the rest of the joint velocities have a cost to some non zero value @@ -299,7 +303,7 @@ TEST_F(CostsTest, equality_jointVel) // NOLINT jv2->first_step = 0; jv2->last_step = pci.basic_info.n_steps - 1; jv2->name = "joint_vel_all"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -383,7 +387,7 @@ TEST_F(CostsTest, inequality_jointVel) // NOLINT jv->first_step = 0; jv->last_step = pci.basic_info.n_steps - 1; jv->name = "joint_vel_limits"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // Joint Velocities also have a cost to some non zero value @@ -395,7 +399,7 @@ TEST_F(CostsTest, inequality_jointVel) // NOLINT jv2->first_step = 0; jv2->last_step = (pci.basic_info.n_steps - 1) / 2; jv2->name = "joint_vel_targ_1"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); auto jv3 = std::make_shared(); @@ -406,7 +410,7 @@ TEST_F(CostsTest, inequality_jointVel) // NOLINT jv3->first_step = (pci.basic_info.n_steps - 1) / 2 + 1; jv3->last_step = pci.basic_info.n_steps - 1; jv3->name = "joint_vel_targ_2"; - jv3->term_type = TT_COST; + jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -494,7 +498,7 @@ TEST_F(CostsTest, equality_jointVel_time) // NOLINT jv->first_step = 0; jv->last_step = 0; jv->name = "joint_vel_single"; - jv->term_type = TT_CNT | TT_USE_TIME; + jv->term_type = TermType::TT_CNT | TermType::TT_USE_TIME; pci.cnt_infos.push_back(jv); // All the rest of the joint velocities have a cost to some non zero value @@ -504,7 +508,7 @@ TEST_F(CostsTest, equality_jointVel_time) // NOLINT jv2->first_step = 0; jv2->last_step = pci.basic_info.n_steps - 1; jv2->name = "joint_vel_all"; - jv2->term_type = TT_COST | TT_USE_TIME; + jv2->term_type = TermType::TT_COST | TermType::TT_USE_TIME; pci.cost_infos.push_back(jv2); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -594,7 +598,7 @@ TEST_F(CostsTest, inequality_jointVel_time) // NOLINT jv->first_step = 0; jv->last_step = pci.basic_info.n_steps - 1; jv->name = "joint_vel_limits"; - jv->term_type = TT_CNT | TT_USE_TIME; + jv->term_type = TermType::TT_CNT | TermType::TT_USE_TIME; pci.cnt_infos.push_back(jv); // Joint Velocities also have a cost to some non zero value @@ -606,7 +610,7 @@ TEST_F(CostsTest, inequality_jointVel_time) // NOLINT jv2->first_step = 0; jv2->last_step = (pci.basic_info.n_steps - 1) / 2; jv2->name = "joint_vel_targ_1"; - jv2->term_type = TT_COST | TT_USE_TIME; + jv2->term_type = TermType::TT_COST | TermType::TT_USE_TIME; pci.cost_infos.push_back(jv2); auto jv3 = std::make_shared(); @@ -617,7 +621,7 @@ TEST_F(CostsTest, inequality_jointVel_time) // NOLINT jv3->first_step = (pci.basic_info.n_steps - 1) / 2 + 1; jv3->last_step = pci.basic_info.n_steps - 1; jv3->name = "joint_vel_targ_2"; - jv3->term_type = TT_COST | TT_USE_TIME; + jv3->term_type = TermType::TT_COST | TermType::TT_USE_TIME; pci.cost_infos.push_back(jv3); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -702,7 +706,7 @@ TEST_F(CostsTest, equality_jointAcc) // NOLINT jv->first_step = 0; jv->last_step = 0; jv->name = "joint_acc_single"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // All the rest of the joint velocities have a cost to some non zero value @@ -712,7 +716,7 @@ TEST_F(CostsTest, equality_jointAcc) // NOLINT jv2->first_step = 0; jv2->last_step = pci.basic_info.n_steps - 1; jv2->name = "joint_acc_all"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); TrajOptProb::Ptr prob = ConstructProblem(pci); @@ -798,7 +802,7 @@ TEST_F(CostsTest, inequality_jointAcc) // NOLINT jv->first_step = 0; jv->last_step = pci.basic_info.n_steps - 1; jv->name = "joint_acc_limits"; - jv->term_type = TT_CNT; + jv->term_type = TermType::TT_CNT; pci.cnt_infos.push_back(jv); // Joint accel also have a cost to some non zero value @@ -810,7 +814,7 @@ TEST_F(CostsTest, inequality_jointAcc) // NOLINT jv2->first_step = 0; jv2->last_step = (pci.basic_info.n_steps - 1) / 2; jv2->name = "joint_acc_targ_1"; - jv2->term_type = TT_COST; + jv2->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv2); auto jv3 = std::make_shared(); @@ -821,7 +825,7 @@ TEST_F(CostsTest, inequality_jointAcc) // NOLINT jv3->first_step = (pci.basic_info.n_steps - 1) / 2 + 1; jv3->last_step = pci.basic_info.n_steps - 1; jv3->name = "joint_acc_targ_2"; - jv3->term_type = TT_COST; + jv3->term_type = TermType::TT_COST; pci.cost_infos.push_back(jv3); TrajOptProb::Ptr prob = ConstructProblem(pci); diff --git a/trajopt/test/kinematic_costs_unit.cpp b/trajopt/test/kinematic_costs_unit.cpp index a4486070..27cdece5 100644 --- a/trajopt/test/kinematic_costs_unit.cpp +++ b/trajopt/test/kinematic_costs_unit.cpp @@ -3,11 +3,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include #include #include diff --git a/trajopt/test/numerical_ik_unit.cpp b/trajopt/test/numerical_ik_unit.cpp index 28275957..5e47a71e 100644 --- a/trajopt/test/numerical_ik_unit.cpp +++ b/trajopt/test/numerical_ik_unit.cpp @@ -4,11 +4,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include #include #include diff --git a/trajopt/test/planning_unit.cpp b/trajopt/test/planning_unit.cpp index 0022a6a5..de47a865 100644 --- a/trajopt/test/planning_unit.cpp +++ b/trajopt/test/planning_unit.cpp @@ -4,12 +4,18 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include +#include #include #include #include diff --git a/trajopt/test/simple_collision_unit.cpp b/trajopt/test/simple_collision_unit.cpp index 1a8d497d..5c1dfef3 100644 --- a/trajopt/test/simple_collision_unit.cpp +++ b/trajopt/test/simple_collision_unit.cpp @@ -2,14 +2,19 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include #include #include +#include +#include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include #include +#include #include #include #include diff --git a/trajopt_common/include/trajopt_common/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h index e67c818c..b1246052 100644 --- a/trajopt_common/include/trajopt_common/collision_types.h +++ b/trajopt_common/include/trajopt_common/collision_types.h @@ -29,11 +29,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_common { using GetStateFn = std::function& joint_values)>; @@ -262,7 +263,5 @@ struct CollisionCacheData std::vector gradient_results_sets; }; -using CollisionCache = Cache; - } // namespace trajopt_common #endif // TRAJOPT_COMMON_COLLISION_TYPES_H diff --git a/trajopt_common/include/trajopt_common/collision_utils.h b/trajopt_common/include/trajopt_common/collision_utils.h index 76ab042b..e7486cd7 100644 --- a/trajopt_common/include/trajopt_common/collision_utils.h +++ b/trajopt_common/include/trajopt_common/collision_utils.h @@ -27,14 +27,16 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_common { +struct TrajOptCollisionConfig; +struct GradientResults; + std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref& dof_vals); std::size_t getHash(const TrajOptCollisionConfig& collision_config, const Eigen::Ref& dof_vals0, @@ -65,7 +67,7 @@ GradientResults getGradient(const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result, double margin, double margin_buffer, - const tesseract_kinematics::JointGroup::ConstPtr& manip); + const tesseract_kinematics::JointGroup& manip); /** * @brief Extracts the gradient information based on the contact results @@ -80,7 +82,7 @@ GradientResults getGradient(const Eigen::VectorXd& dofvals0, const tesseract_collision::ContactResult& contact_result, double margin, double margin_buffer, - const tesseract_kinematics::JointGroup::ConstPtr& manip); + const tesseract_kinematics::JointGroup& manip); /** * @brief Print debug gradient information diff --git a/trajopt_common/include/trajopt_common/fwd.h b/trajopt_common/include/trajopt_common/fwd.h new file mode 100644 index 00000000..c367a88d --- /dev/null +++ b/trajopt_common/include/trajopt_common/fwd.h @@ -0,0 +1,16 @@ +#ifndef TRAJOPT_COMMON_FWD_H +#define TRAJOPT_COMMON_FWD_H + +namespace trajopt_common +{ +struct SafetyMarginData; +struct CollisionCoeffData; +struct TrajOptCollisionConfig; +struct LinkGradientResults; +struct GradientResults; +struct LinkMaxError; +struct GradientResultsSet; +struct CollisionCacheData; +} // namespace trajopt_common + +#endif // TRAJOPT_COMMON_FWD_H diff --git a/trajopt_common/include/trajopt_common/utils.hpp b/trajopt_common/include/trajopt_common/utils.hpp index 62630a5c..3f2bacec 100644 --- a/trajopt_common/include/trajopt_common/utils.hpp +++ b/trajopt_common/include/trajopt_common/utils.hpp @@ -8,6 +8,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TRAJOPT_IGNORE_WARNINGS_POP diff --git a/trajopt_common/src/collision_utils.cpp b/trajopt_common/src/collision_utils.cpp index f99886f9..bfecb250 100644 --- a/trajopt_common/src/collision_utils.cpp +++ b/trajopt_common/src/collision_utils.cpp @@ -26,10 +26,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include #include TRAJOPT_IGNORE_WARNINGS_POP #include +#include namespace trajopt_common { @@ -113,14 +115,14 @@ void calcGradient(GradientResults& results, std::size_t i, const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result, - const tesseract_kinematics::JointGroup::ConstPtr& manip, + const tesseract_kinematics::JointGroup& manip, bool isTimestep1) { LinkGradientResults& link_gradient = (isTimestep1) ? results.cc_gradients[i] : results.gradients[i]; link_gradient.has_gradient = true; // Calculate Jacobian - Eigen::MatrixXd jac = manip->calcJacobian(dofvals, contact_result.link_names[i]); + Eigen::MatrixXd jac = manip.calcJacobian(dofvals, contact_result.link_names[i]); // Need to change the base and ref point of the jacobian. // When changing ref point you must provide a vector from the current ref @@ -167,14 +169,14 @@ GradientResults getGradient(const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result, double margin, double margin_buffer, - const tesseract_kinematics::JointGroup::ConstPtr& manip) + const tesseract_kinematics::JointGroup& manip) { GradientResults results; results.error = (margin - contact_result.distance); results.error_with_buffer = (margin + margin_buffer - contact_result.distance); for (std::size_t i = 0; i < 2; ++i) { - if (manip->isActiveLinkName(contact_result.link_names[i])) + if (manip.isActiveLinkName(contact_result.link_names[i])) calcGradient(results, i, dofvals, contact_result, manip, false); } // DebugPrintInfo(res, results.gradients[0], results.gradients[1], dofvals, &res == &(dist_results.front())); @@ -187,7 +189,7 @@ GradientResults getGradient(const Eigen::VectorXd& dofvals0, const tesseract_collision::ContactResult& contact_result, double margin, double margin_buffer, - const tesseract_kinematics::JointGroup::ConstPtr& manip) + const tesseract_kinematics::JointGroup& manip) { GradientResults results; results.error = (margin - contact_result.distance); @@ -196,7 +198,7 @@ GradientResults getGradient(const Eigen::VectorXd& dofvals0, Eigen::VectorXd dofvalst = Eigen::VectorXd::Zero(dofvals0.size()); for (std::size_t i = 0; i < 2; ++i) { - if (manip->isActiveLinkName(contact_result.link_names[i])) + if (manip.isActiveLinkName(contact_result.link_names[i])) { if (contact_result.cc_type[i] == tesseract_collision::ContinuousCollisionType::CCType_Time0) dofvalst = dofvals0; diff --git a/trajopt_common/src/utils.cpp b/trajopt_common/src/utils.cpp index 82e7f6cf..b709154b 100644 --- a/trajopt_common/src/utils.cpp +++ b/trajopt_common/src/utils.cpp @@ -43,7 +43,10 @@ const std::array& SafetyMarginData::getPairSafetyMarginData(const std double SafetyMarginData::getMaxSafetyMargin() const { return max_safety_margin_; } -const std::set& SafetyMarginData::getPairsWithZeroCoeff() const { return zero_coeff_; } +const std::set>& SafetyMarginData::getPairsWithZeroCoeff() const +{ + return zero_coeff_; +} std::vector createSafetyMarginDataVector(int num_elements, double default_safety_margin, diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h index 0d5ed8ca..56f862da 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h @@ -30,18 +30,15 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include #include - -#include -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief Contains kinematic information for the cartesian position cost; include cart point .h & remove? */ @@ -54,7 +51,7 @@ struct CartLineInfo CartLineInfo() = default; CartLineInfo( - tesseract_kinematics::JointGroup::ConstPtr manip, + std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& target_frame_offset1, @@ -63,7 +60,7 @@ struct CartLineInfo const Eigen::VectorXi& indices = Eigen::Matrix(std::vector({ 0, 1, 2, 3, 4, 5 }).data())); /** @brief The joint group */ - tesseract_kinematics::JointGroup::ConstPtr manip; + std::shared_ptr manip; /** @brief Link which should reach desired pos */ std::string source_frame; @@ -103,7 +100,7 @@ class CartLineConstraint : public ifopt::ConstraintSet std::function; CartLineConstraint(CartLineInfo info, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, const std::string& name = "CartLine"); @@ -188,7 +185,7 @@ class CartLineConstraint : public ifopt::ConstraintSet * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; /** @brief The cartesian line information used when calculating error */ CartLineInfo info_; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h index d5f4d3bf..2b963813 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h @@ -31,16 +31,13 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include - -#include -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** @brief Contains Cartesian pose constraint information */ struct CartPosInfo { @@ -57,7 +54,7 @@ struct CartPosInfo }; CartPosInfo() = default; - CartPosInfo(tesseract_kinematics::JointGroup::ConstPtr manip, + CartPosInfo(std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset = Eigen::Isometry3d::Identity(), @@ -65,7 +62,7 @@ struct CartPosInfo const Eigen::VectorXi& indices = Eigen::Matrix(std::vector({ 0, 1, 2, 3, 4, 5 }).data())); /** @brief The joint group */ - tesseract_kinematics::JointGroup::ConstPtr manip; + std::shared_ptr manip; /** @brief Link which should reach desired pos */ std::string source_frame; @@ -102,10 +99,12 @@ class CartPosConstraint : public ifopt::ConstraintSet using ErrorDiffFunctionType = std::function; - CartPosConstraint(CartPosInfo info, JointPosition::ConstPtr position_var, const std::string& name = "CartPos"); + CartPosConstraint(CartPosInfo info, + std::shared_ptr position_var, + const std::string& name = "CartPos"); CartPosConstraint(CartPosInfo info, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, const std::string& name = "CartPos"); @@ -189,7 +188,7 @@ class CartPosConstraint : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; /** @brief The kinematic information used when calculating error */ CartPosInfo info_; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h index 0ef52f8a..af91340f 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h @@ -26,18 +26,18 @@ */ #ifndef TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H #define TRAJOPT_IFOPT_CONTINUOUS_COLLISION_CONSTRAINT_V2_H + #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include #include -#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include - namespace trajopt_ifopt { +class JointPosition; +class ContinuousCollisionEvaluator; + class ContinuousCollisionConstraint : public ifopt::ConstraintSet { public: @@ -53,8 +53,8 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet * @param fixed_sparsity This is mostly need for snopt which requires sparsity to not change * @param name */ - ContinuousCollisionConstraint(ContinuousCollisionEvaluator::Ptr collision_evaluator, - std::array position_vars, + ContinuousCollisionConstraint(std::shared_ptr collision_evaluator, + std::array, 2> position_vars, std::array position_vars_fixed, int max_num_cnt = 1, bool fixed_sparsity = false, @@ -95,7 +95,7 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet * @brief Get the collision evaluator. This exposed for plotter callbacks * @return The collision evaluator */ - ContinuousCollisionEvaluator::Ptr GetCollisionEvaluator() const; + std::shared_ptr GetCollisionEvaluator() const; private: /** @brief The number of joints in a single JointPosition */ @@ -108,13 +108,13 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - std::array position_vars_; + std::array, 2> position_vars_; std::array position_vars_fixed_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ std::vector> triplet_list_; - ContinuousCollisionEvaluator::Ptr collision_evaluator_; + std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h index 9bb50c07..3908a6f7 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_evaluators.h @@ -27,19 +27,24 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include - -#include -#include -#include -#include +#include +#include +#include + +#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace trajopt_ifopt { +using GetStateFn = std::function& joint_values)>; +using CollisionCache = trajopt_common::Cache>; + /** * @brief This collision evaluator operates on two states and checks for collision between the two states using a * casted collision objects between to intermediate interpolated states. @@ -66,7 +71,7 @@ class ContinuousCollisionEvaluator * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the * data. */ - virtual trajopt_common::CollisionCacheData::ConstPtr + virtual std::shared_ptr CalcCollisionData(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const std::array& position_vars_fixed, @@ -105,16 +110,17 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - LVSContinuousCollisionEvaluator(std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + LVSContinuousCollisionEvaluator(std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment = false); - trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const std::array& position_vars_fixed, - std::size_t bounds_size) override final; + std::shared_ptr + CalcCollisionData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const std::array& position_vars_fixed, + std::size_t bounds_size) override final; trajopt_common::GradientResults CalcGradientData(const Eigen::Ref& dof_vals0, @@ -124,18 +130,18 @@ class LVSContinuousCollisionEvaluator : public ContinuousCollisionEvaluator const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final; private: - std::shared_ptr collision_cache_; - tesseract_kinematics::JointGroup::ConstPtr manip_; - tesseract_environment::Environment::ConstPtr env_; - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; + std::shared_ptr collision_cache_; + std::shared_ptr manip_; + std::shared_ptr env_; + std::shared_ptr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - trajopt_common::GetStateFn get_state_fn_; + GetStateFn get_state_fn_; bool dynamic_environment_; - tesseract_collision::ContinuousContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; - trajopt_common::CollisionCacheData::ConstPtr + std::shared_ptr CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1); @@ -157,16 +163,17 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - LVSDiscreteCollisionEvaluator(std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + LVSDiscreteCollisionEvaluator(std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment = false); - trajopt_common::CollisionCacheData::ConstPtr CalcCollisionData(const Eigen::Ref& dof_vals0, - const Eigen::Ref& dof_vals1, - const std::array& position_vars_fixed, - std::size_t bounds_size) override final; + std::shared_ptr + CalcCollisionData(const Eigen::Ref& dof_vals0, + const Eigen::Ref& dof_vals1, + const std::array& position_vars_fixed, + std::size_t bounds_size) override final; trajopt_common::GradientResults CalcGradientData(const Eigen::Ref& dof_vals0, @@ -176,18 +183,18 @@ class LVSDiscreteCollisionEvaluator : public ContinuousCollisionEvaluator const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override final; private: - std::shared_ptr collision_cache_; - tesseract_kinematics::JointGroup::ConstPtr manip_; - tesseract_environment::Environment::ConstPtr env_; - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; + std::shared_ptr collision_cache_; + std::shared_ptr manip_; + std::shared_ptr env_; + std::shared_ptr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - trajopt_common::GetStateFn get_state_fn_; + GetStateFn get_state_fn_; bool dynamic_environment_; - tesseract_collision::DiscreteContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; - trajopt_common::CollisionCacheData::ConstPtr + std::shared_ptr CalcCollisionsCacheDataHelper(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1); diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h index 969630f6..bf8fc9aa 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h @@ -30,14 +30,13 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include - namespace trajopt_ifopt { +class JointPosition; +class ContinuousCollisionEvaluator; + class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet { public: @@ -53,8 +52,8 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet * @param fixed_sparsity This is mostly need for snopt which requires sparsity to not change * @param name */ - ContinuousCollisionNumericalConstraint(ContinuousCollisionEvaluator::Ptr collision_evaluator, - std::array position_vars, + ContinuousCollisionNumericalConstraint(std::shared_ptr collision_evaluator, + std::array, 2> position_vars, std::array position_vars_fixed, int max_num_cnt = 1, bool fixed_sparsity = false, @@ -95,7 +94,7 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet * @brief Get the collision evaluator. This exposed for plotter callbacks * @return The collision evaluator */ - ContinuousCollisionEvaluator::Ptr GetCollisionEvaluator() const; + std::shared_ptr GetCollisionEvaluator() const; private: /** @brief The number of joints in a single JointPosition */ @@ -108,13 +107,13 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - std::array position_vars_; + std::array, 2> position_vars_; std::array position_vars_fixed_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ std::vector> triplet_list_; - ContinuousCollisionEvaluator::Ptr collision_evaluator_; + std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt #endif // TRAJOPT_IFOPT_CONTINUOUS_COLLISION_NUMERICAL_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h index 7d5c9aba..13da1948 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h @@ -29,24 +29,23 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include - namespace trajopt_ifopt { +class JointPosition; +class DiscreteCollisionEvaluator; + class DiscreteCollisionConstraint : public ifopt::ConstraintSet { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - DiscreteCollisionConstraint(DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, + DiscreteCollisionConstraint(std::shared_ptr collision_evaluator, + std::shared_ptr position_var, int max_num_cnt = 1, bool fixed_sparsity = false, const std::string& name = "DiscreteCollisionV3"); @@ -96,7 +95,7 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet * @brief Get the collision evaluator. This exposed for plotter callbacks * @return The collision evaluator */ - DiscreteCollisionEvaluator::Ptr GetCollisionEvaluator() const; + std::shared_ptr GetCollisionEvaluator() const; private: /** @brief The number of joints in a single JointPosition */ @@ -109,12 +108,12 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ std::vector> triplet_list_; - DiscreteCollisionEvaluator::Ptr collision_evaluator_; + std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h index b355efef..a8f3fddf 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_evaluators.h @@ -26,19 +26,24 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include - -#include -#include -#include -#include +#include +#include +#include + +#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include +#include +#include namespace trajopt_ifopt { +using GetStateFn = std::function& joint_values)>; +using CollisionCache = trajopt_common::Cache>; + /** * @brief This collision evaluator only operates on a single state in the trajectory and does not check for collisions * between states. @@ -64,8 +69,8 @@ class DiscreteCollisionEvaluator * @return Collision cache data. If a cache does not exist for the provided joint values it evaluates and stores the * data. */ - virtual trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, - std::size_t bounds_size) = 0; + virtual std::shared_ptr + CalcCollisions(const Eigen::Ref& dof_vals, std::size_t bounds_size) = 0; /** * @brief Get the safety margin information. @@ -96,14 +101,14 @@ class SingleTimestepCollisionEvaluator : public DiscreteCollisionEvaluator using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SingleTimestepCollisionEvaluator(std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + SingleTimestepCollisionEvaluator(std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment = false); - trajopt_common::CollisionCacheData::ConstPtr CalcCollisions(const Eigen::Ref& dof_vals, - std::size_t bounds_size) override final; + std::shared_ptr + CalcCollisions(const Eigen::Ref& dof_vals, std::size_t bounds_size) override final; trajopt_common::GradientResults GetGradient(const Eigen::VectorXd& dofvals, const tesseract_collision::ContactResult& contact_result) override; @@ -111,16 +116,16 @@ class SingleTimestepCollisionEvaluator : public DiscreteCollisionEvaluator const trajopt_common::TrajOptCollisionConfig& GetCollisionConfig() const override; private: - std::shared_ptr collision_cache_; - tesseract_kinematics::JointGroup::ConstPtr manip_; - tesseract_environment::Environment::ConstPtr env_; - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config_; + std::shared_ptr collision_cache_; + std::shared_ptr manip_; + std::shared_ptr env_; + std::shared_ptr collision_config_; std::vector env_active_link_names_; std::vector manip_active_link_names_; std::vector diff_active_link_names_; - trajopt_common::GetStateFn get_state_fn_; + GetStateFn get_state_fn_; bool dynamic_environment_; - tesseract_collision::DiscreteContactManager::Ptr contact_manager_; + std::shared_ptr contact_manager_; void CalcCollisionsHelper(const Eigen::Ref& dof_vals, tesseract_collision::ContactResultMap& dist_results); diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h index 198edce7..9c18d822 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h @@ -32,20 +32,19 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include -#include -#include - namespace trajopt_ifopt { +class JointPosition; +class DiscreteCollisionEvaluator; + class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - DiscreteCollisionNumericalConstraint(DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, + DiscreteCollisionNumericalConstraint(std::shared_ptr collision_evaluator, + std::shared_ptr position_var, int max_num_cnt = 1, bool fixed_sparsity = false, const std::string& name = "DiscreteCollisionNumerical"); @@ -95,7 +94,7 @@ class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet * @brief Get the collision evaluator. This exposed for plotter callbacks * @return The collision evaluator */ - DiscreteCollisionEvaluator::Ptr GetCollisionEvaluator() const; + std::shared_ptr GetCollisionEvaluator() const; private: /** @brief The number of joints in a single JointPosition */ @@ -108,12 +107,12 @@ class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ std::vector> triplet_list_; - DiscreteCollisionEvaluator::Ptr collision_evaluator_; + std::shared_ptr collision_evaluator_; }; } // namespace trajopt_ifopt #endif // TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h index 62a0fd54..970d3096 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/weighted_average_methods.h @@ -26,10 +26,10 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h index c8fea0cb..05bb8dc8 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h @@ -30,16 +30,13 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include - -#include -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief Contains kinematic information for the inverse kinematics constraint */ @@ -51,12 +48,12 @@ struct InverseKinematicsInfo using ConstPtr = std::shared_ptr; InverseKinematicsInfo() = default; - InverseKinematicsInfo(tesseract_kinematics::KinematicGroup::ConstPtr manip, + InverseKinematicsInfo(std::shared_ptr manip, std::string working_frame, std::string tcp_frame, const Eigen::Isometry3d& tcp_offset = Eigen::Isometry3d::Identity()); - tesseract_kinematics::KinematicGroup::ConstPtr manip; + std::shared_ptr manip; /** @brief Not currently respected */ std::string working_frame; @@ -86,8 +83,8 @@ class InverseKinematicsConstraint : public ifopt::ConstraintSet InverseKinematicsConstraint(const Eigen::Isometry3d& target_pose, InverseKinematicsInfo::ConstPtr kinematic_info, - JointPosition::ConstPtr constraint_var, - JointPosition::ConstPtr seed_var, + std::shared_ptr constraint_var, + std::shared_ptr seed_var, const std::string& name = "InverseKinematics"); /** @@ -151,10 +148,10 @@ class InverseKinematicsConstraint : public ifopt::ConstraintSet /** @brief Pointer to the var used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - JointPosition::ConstPtr constraint_var_; + std::shared_ptr constraint_var_; /** @brief Pointer to the var used as a seed when calculating IK. This will usually be a adjacent point in the * trajectory*/ - JointPosition::ConstPtr seed_var_; + std::shared_ptr seed_var_; /** @brief Target pose for the TCP. Currently in robot frame since world_to_base_ has not been implemented */ Eigen::Isometry3d target_pose_; /** @brief The kinematic info used to create this constraint */ diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h index 0868e10b..714ea84e 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h @@ -29,13 +29,13 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position * @@ -56,7 +56,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointAccelConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointAccel"); @@ -69,7 +69,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointAccelConstraint(const std::vector& bounds, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointAccel"); @@ -107,7 +107,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet /** @brief Pointers to the vars used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector position_vars_; + std::vector> position_vars_; std::unordered_map index_map_; }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h index cb7a4936..b7df92cb 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h @@ -29,13 +29,13 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position * @@ -56,7 +56,7 @@ class JointJerkConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointJerkConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointJerk"); @@ -69,7 +69,7 @@ class JointJerkConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointJerkConstraint(const std::vector& bounds, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointJerk"); @@ -107,8 +107,8 @@ class JointJerkConstraint : public ifopt::ConstraintSet /** @brief Pointers to the vars used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector position_vars_; + std::vector> position_vars_; std::unordered_map index_map_; }; -}; // namespace trajopt_ifopt +} // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h index 0c996381..a5f16146 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h @@ -29,13 +29,13 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief This creates a joint position constraint. Allows bounds to be set on a joint position */ @@ -54,7 +54,7 @@ class JointPosConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointPosConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointPos"); @@ -66,7 +66,7 @@ class JointPosConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointPosConstraint(const std::vector& bounds, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointPos"); @@ -105,7 +105,7 @@ class JointPosConstraint : public ifopt::ConstraintSet /** @brief Pointers to the vars used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector position_vars_; + std::vector> position_vars_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h index 83985477..4dc3bf93 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h @@ -29,13 +29,13 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include - namespace trajopt_ifopt { +class JointPosition; + /** * @brief This creates a joint velocity constraint and allows bounds to be set on a joint position * @@ -56,7 +56,7 @@ class JointVelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointVelConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointVel"); @@ -69,7 +69,7 @@ class JointVelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointVelConstraint(const std::vector& bounds, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointVel"); @@ -107,7 +107,7 @@ class JointVelConstraint : public ifopt::ConstraintSet /** @brief Pointers to the vars used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector position_vars_; + std::vector> position_vars_; std::unordered_map index_map_; }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/include/trajopt_ifopt/fwd.h b/trajopt_ifopt/include/trajopt_ifopt/fwd.h new file mode 100644 index 00000000..5137178a --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/fwd.h @@ -0,0 +1,67 @@ +/** + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TRAJOPT_IFOPT_FWD_H +#define TRAJOPT_IFOPT_FWD_H + +namespace trajopt_ifopt +{ +// joint_position_variable.h +class JointPosition; + +// cartesian_line_constraint.h +struct CartLineInfo; +class CartLineConstraint; + +// cartesian_position_constraint.h +struct CartPosInfo; +class CartPosConstraint; + +// inverse_kinematics_constraint.h +struct InverseKinematicsInfo; +class InverseKinematicsConstraint; + +// joint_acceleration_constraint.h +class JointAccelConstraint; + +// joint_jerk_constraint.h +class JointJerkConstraint; + +// joint_position_constraint.h +class JointPosConstraint; + +// joint_velocity_constraint.h +class JointVelConstraint; + +// continuous_collision_evaluators.h +class ContinuousCollisionEvaluator; +class LVSContinuousCollisionEvaluator; +class LVSDiscreteCollisionEvaluator; + +// discrete_collision_evaluators.h +class DiscreteCollisionEvaluator; +class SingleTimestepCollisionEvaluator; + +// continuous_collision_constraint.h +class ContinuousCollisionConstraint; +class ContinuousCollisionNumericalConstraint; + +// discrete_collision_constraint.h +class DiscreteCollisionConstraint; +class DiscreteCollisionNumericalConstraint; + +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_FWD_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h b/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h deleted file mode 100644 index 535e1d28..00000000 --- a/trajopt_ifopt/include/trajopt_ifopt/trajopt_ifopt.h +++ /dev/null @@ -1,49 +0,0 @@ -/** - * @file trajopt_ifopt.h - * @brief Includes all of the trajopt_ifopt headers - * - * @author Matthew Powelson - * @date September 8, 2020 - * @version TODO - * @bug No known bugs - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TRAJOPT_IFOPT_TRAJOPT_IFOPT_H -#define TRAJOPT_IFOPT_TRAJOPT_IFOPT_H - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include - -#endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h b/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h index 45d3f36b..a9c2fbe4 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h +++ b/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h @@ -28,20 +28,21 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include namespace trajopt_ifopt { +class JointPosition; + /** * @brief Converts a vector of trajopt variables into the legacy TrajArray format * @param joint_positions Vector of joint positions. Must be in order and all the same length * @return TrajArray - size = [joint_positions.size(), joint_positions.n_dof] */ -tesseract_common::TrajArray toTrajArray(const std::vector& joint_positions); +tesseract_common::TrajArray toTrajArray(const std::vector>& joint_positions); /** * @brief Converts a vector of trajopt variables into tesseract_common JointTrajectory @@ -49,7 +50,7 @@ tesseract_common::TrajArray toTrajArray(const std::vector& joint_positions); +toJointTrajectory(const std::vector>& joint_positions); } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h index 4835b90a..270dbfc8 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h @@ -30,8 +30,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include -#include -#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp index ede76b2c..20123c32 100644 --- a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp @@ -25,18 +25,22 @@ * See the License for the specific language governing permissions and * limitations under the License. */ + #include +#include #include #include #include + TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -CartLineInfo::CartLineInfo(tesseract_kinematics::JointGroup::ConstPtr manip, +CartLineInfo::CartLineInfo(std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& target_frame_offset1, // NOLINT(modernize-pass-by-value) @@ -68,7 +72,7 @@ CartLineInfo::CartLineInfo(tesseract_kinematics::JointGroup::ConstPtr manip, } CartLineConstraint::CartLineConstraint(CartLineInfo info, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, // NOLINT const std::string& name) : ifopt::ConstraintSet(static_cast(info.indices.rows()), name) diff --git a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp index 0893bdda..bcdfc08a 100644 --- a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp @@ -25,18 +25,20 @@ * limitations under the License. */ #include +#include #include #include #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -CartPosInfo::CartPosInfo(tesseract_kinematics::JointGroup::ConstPtr manip, +CartPosInfo::CartPosInfo(std::shared_ptr manip, std::string source_frame, std::string target_frame, const Eigen::Isometry3d& source_frame_offset, // NOLINT(modernize-pass-by-value) @@ -74,7 +76,7 @@ CartPosInfo::CartPosInfo(tesseract_kinematics::JointGroup::ConstPtr manip, } CartPosConstraint::CartPosConstraint(CartPosInfo info, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, // NOLINT const std::string& name) : ifopt::ConstraintSet(static_cast(info.indices.rows()), name) @@ -191,7 +193,9 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, } } -CartPosConstraint::CartPosConstraint(CartPosInfo info, JointPosition::ConstPtr position_var, const std::string& name) +CartPosConstraint::CartPosConstraint(CartPosInfo info, + std::shared_ptr position_var, + const std::string& name) : CartPosConstraint(std::move(info), std::move(position_var), Eigen::VectorXd::Ones(info.indices.rows()), name) { } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp index 9a4b1957..1e4498fc 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -27,22 +27,24 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include +#include +#include namespace trajopt_ifopt { -ContinuousCollisionConstraint::ContinuousCollisionConstraint(ContinuousCollisionEvaluator::Ptr collision_evaluator, - std::array position_vars, - std::array position_vars_fixed, - int max_num_cnt, - bool fixed_sparsity, - const std::string& name) +ContinuousCollisionConstraint::ContinuousCollisionConstraint( + std::shared_ptr collision_evaluator, + std::array, 2> position_vars, + std::array position_vars_fixed, + int max_num_cnt, + bool fixed_sparsity, + const std::string& name) : ifopt::ConstraintSet(max_num_cnt, name) , position_vars_(std::move(position_vars)) , position_vars_fixed_(position_vars_fixed) @@ -195,7 +197,7 @@ void ContinuousCollisionConstraint::SetBounds(const std::vector& bounds_ = bounds; } -ContinuousCollisionEvaluator::Ptr ContinuousCollisionConstraint::GetCollisionEvaluator() const +std::shared_ptr ContinuousCollisionConstraint::GetCollisionEvaluator() const { return collision_evaluator_; } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp index 614219a5..219facf8 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_evaluators.cpp @@ -23,15 +23,24 @@ */ #include +#include #include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + namespace trajopt_ifopt { LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( - std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) @@ -77,7 +86,7 @@ LVSContinuousCollisionEvaluator::LVSContinuousCollisionEvaluator( collision_config_->collision_margin_buffer); } -trajopt_common::CollisionCacheData::ConstPtr +std::shared_ptr LVSContinuousCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const std::array& position_vars_fixed, @@ -258,7 +267,7 @@ LVSContinuousCollisionEvaluator::CalcGradientData(const Eigen::Refcollision_margin_buffer, manip_); + dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, *manip_); } const trajopt_common::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::GetCollisionConfig() const @@ -269,10 +278,10 @@ const trajopt_common::TrajOptCollisionConfig& LVSContinuousCollisionEvaluator::G ////////////////////////////////////////// LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( - std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) @@ -315,7 +324,7 @@ LVSDiscreteCollisionEvaluator::LVSDiscreteCollisionEvaluator( collision_config_->collision_margin_buffer); } -trajopt_common::CollisionCacheData::ConstPtr +std::shared_ptr LVSDiscreteCollisionEvaluator::CalcCollisionData(const Eigen::Ref& dof_vals0, const Eigen::Ref& dof_vals1, const std::array& position_vars_fixed, @@ -486,7 +495,7 @@ LVSDiscreteCollisionEvaluator::CalcGradientData(const Eigen::Refcollision_margin_buffer, manip_); + dof_vals0, dof_vals1, contact_results, margin, collision_config_->collision_margin_buffer, *manip_); } const trajopt_common::TrajOptCollisionConfig& LVSDiscreteCollisionEvaluator::GetCollisionConfig() const diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp index 8a25f054..acc5270a 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp @@ -27,19 +27,19 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include +#include #include namespace trajopt_ifopt { ContinuousCollisionNumericalConstraint::ContinuousCollisionNumericalConstraint( - ContinuousCollisionEvaluator::Ptr collision_evaluator, - std::array position_vars, + std::shared_ptr collision_evaluator, + std::array, 2> position_vars, std::array position_vars_fixed, int max_num_cnt, bool fixed_sparsity, @@ -201,7 +201,7 @@ void ContinuousCollisionNumericalConstraint::SetBounds(const std::vector ContinuousCollisionNumericalConstraint::GetCollisionEvaluator() const { return collision_evaluator_; } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp index b8c95e61..f39ffb8b 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -27,22 +27,24 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include +#include #include +#include +#include #include namespace trajopt_ifopt { -DiscreteCollisionConstraint::DiscreteCollisionConstraint(DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, - int max_num_cnt, - bool fixed_sparsity, - const std::string& name) +DiscreteCollisionConstraint::DiscreteCollisionConstraint( + std::shared_ptr collision_evaluator, + std::shared_ptr position_var, + int max_num_cnt, + bool fixed_sparsity, + const std::string& name) : ifopt::ConstraintSet(max_num_cnt, name) , position_var_(std::move(position_var)) , collision_evaluator_(std::move(collision_evaluator)) @@ -142,7 +144,7 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Ref DiscreteCollisionConstraint::GetCollisionEvaluator() const { return collision_evaluator_; } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp index bf814982..297fc43a 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_evaluators.cpp @@ -23,15 +23,24 @@ */ #include +#include #include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + namespace trajopt_ifopt { SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( - std::shared_ptr collision_cache, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_environment::Environment::ConstPtr env, - trajopt_common::TrajOptCollisionConfig::ConstPtr collision_config, + std::shared_ptr collision_cache, + std::shared_ptr manip, + std::shared_ptr env, + std::shared_ptr collision_config, bool dynamic_environment) : collision_cache_(std::move(collision_cache)) , manip_(std::move(manip)) @@ -74,7 +83,7 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator( collision_config_->collision_margin_buffer); } -trajopt_common::CollisionCacheData::ConstPtr +std::shared_ptr SingleTimestepCollisionEvaluator::CalcCollisions(const Eigen::Ref& dof_vals, std::size_t bounds_size) { @@ -190,7 +199,7 @@ SingleTimestepCollisionEvaluator::GetGradient(const Eigen::VectorXd& dofvals, contact_result.link_names[0], contact_result.link_names[1]); return trajopt_common::getGradient( - dofvals, contact_result, margin, collision_config_->collision_margin_buffer, manip_); + dofvals, contact_result, margin, collision_config_->collision_margin_buffer, *manip_); } const trajopt_common::TrajOptCollisionConfig& SingleTimestepCollisionEvaluator::GetCollisionConfig() const diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp index d821ecd5..90f9d9ec 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp @@ -27,20 +27,20 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include +#include #include -#include namespace trajopt_ifopt { DiscreteCollisionNumericalConstraint::DiscreteCollisionNumericalConstraint( - DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, + std::shared_ptr collision_evaluator, + std::shared_ptr position_var, int max_num_cnt, bool fixed_sparsity, const std::string& name) @@ -178,7 +178,7 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Ref DiscreteCollisionNumericalConstraint::GetCollisionEvaluator() const { return collision_evaluator_; } diff --git a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp index be6bde20..49754950 100644 --- a/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp +++ b/trajopt_ifopt/src/constraints/collision/weighted_average_methods.cpp @@ -23,6 +23,7 @@ */ #include +#include namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp index 62ee2e81..5dff8e32 100644 --- a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp +++ b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp @@ -24,15 +24,16 @@ * limitations under the License. */ #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -InverseKinematicsInfo::InverseKinematicsInfo(tesseract_kinematics::KinematicGroup::ConstPtr manip, +InverseKinematicsInfo::InverseKinematicsInfo(std::shared_ptr manip, std::string working_frame, std::string tcp_frame, const Eigen::Isometry3d& tcp_offset) // NOLINT(modernize-pass-by-value) @@ -48,8 +49,8 @@ InverseKinematicsInfo::InverseKinematicsInfo(tesseract_kinematics::KinematicGrou InverseKinematicsConstraint::InverseKinematicsConstraint( const Eigen::Isometry3d& target_pose, // NOLINT(modernize-pass-by-value) InverseKinematicsInfo::ConstPtr kinematic_info, - JointPosition::ConstPtr constraint_var, - JointPosition::ConstPtr seed_var, + std::shared_ptr constraint_var, + std::shared_ptr seed_var, const std::string& name) : ifopt::ConstraintSet(constraint_var->GetRows(), name) , constraint_var_(std::move(constraint_var)) diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp index 1f3bac9b..3c64b350 100644 --- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp @@ -24,6 +24,7 @@ * limitations under the License. */ #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -32,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector >& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp index f110fc8e..317a3086 100644 --- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp @@ -24,6 +24,7 @@ * limitations under the License. */ #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -32,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) @@ -121,7 +122,7 @@ void JointJerkConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_b Eigen::Index i = it->second; // Reserve enough room in the sparse matrix - std::vector > triplet_list; + std::vector> triplet_list; triplet_list.reserve(static_cast(n_dof_ * 4)); // jac block will be (n_vars-1)*n_dof x n_dof diff --git a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp index a3b8625f..81b6a3db 100644 --- a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp @@ -24,6 +24,7 @@ * limitations under the License. */ #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -32,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) @@ -76,7 +77,7 @@ JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets, } JointPosConstraint::JointPosConstraint(const std::vector& bounds, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(bounds.size()) * static_cast(position_vars.size()), name) @@ -129,7 +130,7 @@ void JointPosConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_bl if (var_set == position_vars_[static_cast(i)]->GetName()) // NOLINT { // Reserve enough room in the sparse matrix - std::vector > triplet_list; + std::vector> triplet_list; triplet_list.reserve(static_cast(n_dof_)); // Each jac_block will be for a single variable but for all timesteps. Therefore we must index down to the diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp index 39f8e211..5001dd80 100644 --- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp @@ -24,6 +24,7 @@ * limitations under the License. */ #include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -32,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointVelConstraint::JointVelConstraint(const Eigen::VectorXd& targets, - const std::vector& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size() - 1), name) @@ -119,7 +120,7 @@ void JointVelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_bl Eigen::Index i = it->second; // Reserve enough room in the sparse matrix - std::vector > triplet_list; + std::vector> triplet_list; triplet_list.reserve(static_cast(3 * n_dof_)); for (int j = 0; j < n_dof_; j++) diff --git a/trajopt_ifopt/src/utils/trajopt_utils.cpp b/trajopt_ifopt/src/utils/trajopt_utils.cpp index a161f00d..e0182d79 100644 --- a/trajopt_ifopt/src/utils/trajopt_utils.cpp +++ b/trajopt_ifopt/src/utils/trajopt_utils.cpp @@ -25,6 +25,11 @@ */ #include +#include + +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { diff --git a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp b/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp index a5dddaa9..ec58bb79 100644 --- a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp +++ b/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp @@ -26,6 +26,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp index 10456dd8..e14a0e84 100644 --- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp @@ -6,9 +6,13 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include using namespace trajopt_ifopt; diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp index fd5df119..85d3692d 100644 --- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp @@ -30,7 +30,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include - +#include #include #include #include @@ -39,6 +39,7 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include +#include #include using namespace trajopt_ifopt; diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp index e6e3dd15..82d4fa47 100644 --- a/trajopt_ifopt/test/cast_cost_unit.cpp +++ b/trajopt_ifopt/test/cast_cost_unit.cpp @@ -28,19 +28,26 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include +#include #include #include #include #include -TRAJOPT_IGNORE_WARNINGS_POP - +#include #include #include #include #include +TRAJOPT_IGNORE_WARNINGS_POP + #include #include #include +#include #include using namespace trajopt_ifopt; @@ -141,7 +148,7 @@ TEST_F(CastTest, boxes) // NOLINT nlp.AddConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < (vars.size()); ++i) { diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp index 1b6d6601..3c33270e 100644 --- a/trajopt_ifopt/test/collision_unit.cpp +++ b/trajopt_ifopt/test/collision_unit.cpp @@ -28,13 +28,19 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include #include #include #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include +#include using namespace trajopt_ifopt; using namespace tesseract_environment; @@ -63,7 +69,7 @@ class CollisionUnit : public testing::TestWithParam // Set up collision evaluator tesseract_kinematics::JointGroup::ConstPtr kin = env->getJointGroup("manipulator"); auto config = std::make_shared(0.1, 1); - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); collision_evaluator = std::make_shared(collision_cache, kin, env, config); diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp index 52be34dc..240d45a8 100644 --- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp @@ -28,19 +28,26 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include +#include #include #include #include #include -TRAJOPT_IGNORE_WARNINGS_POP - +#include #include #include #include #include +TRAJOPT_IGNORE_WARNINGS_POP + #include #include #include +#include #include #include @@ -126,7 +133,7 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) trajopt_collision_config->type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_collision_config->collision_margin_buffer = 0.05; - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); for (std::size_t i = 1; i < vars.size(); ++i) { auto collision_evaluator = std::make_shared( diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp index a10fd528..d14b108b 100644 --- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp @@ -28,10 +28,16 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include @@ -41,6 +47,7 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include +#include #include using namespace trajopt_ifopt; @@ -104,7 +111,7 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.0; // 0.05 - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); auto cnt = std::make_shared(collision_evaluator, vars[0], 3); diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp index b05b5bc0..4a3cb088 100644 --- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp +++ b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp @@ -30,7 +30,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include - +#include #include #include #include @@ -39,6 +39,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include using namespace trajopt_ifopt; diff --git a/trajopt_ifopt/test/joint_terms_unit.cpp b/trajopt_ifopt/test/joint_terms_unit.cpp index e69f79a1..fa2dd282 100644 --- a/trajopt_ifopt/test/joint_terms_unit.cpp +++ b/trajopt_ifopt/test/joint_terms_unit.cpp @@ -28,13 +28,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP #include #include #include #include +#include #include -#include using namespace trajopt_ifopt; using namespace std; diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp index c9814368..6477e10d 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/test/simple_collision_unit.cpp @@ -28,13 +28,20 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include #include #include #include #include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include +#include #include #include #include @@ -104,7 +111,7 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT auto trajopt_collision_config = std::make_shared(margin, margin_coeff); trajopt_collision_config->collision_margin_buffer = 0.05; - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); diff --git a/trajopt_optimizers/trajopt_sqp/CMakeLists.txt b/trajopt_optimizers/trajopt_sqp/CMakeLists.txt index 57595f11..3bd7f051 100644 --- a/trajopt_optimizers/trajopt_sqp/CMakeLists.txt +++ b/trajopt_optimizers/trajopt_sqp/CMakeLists.txt @@ -44,7 +44,8 @@ add_library( src/ifopt_qp_problem.cpp src/trust_region_sqp_solver.cpp src/trajopt_qp_problem.cpp - src/expressions.cpp) + src/expressions.cpp + src/types.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC console_bridge::console_bridge diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h index d60aaff4..d3424661 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/cartesian_error_plotter.h @@ -28,14 +28,12 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include namespace trajopt_sqp { @@ -43,7 +41,7 @@ namespace trajopt_sqp * @brief SQPCallback the error of a CartPosConstraint. It plots an axis at the target and the current position with an * arrow between them */ -class CartesianErrorPlottingCallback : public trajopt_sqp::SQPCallback +class CartesianErrorPlottingCallback : public SQPCallback { public: using Ptr = std::shared_ptr; @@ -53,7 +51,7 @@ class CartesianErrorPlottingCallback : public trajopt_sqp::SQPCallback * @brief Callback constructor * @param plotter Plotter used to plot the error */ - CartesianErrorPlottingCallback(tesseract_visualization::Visualization::Ptr plotter); + CartesianErrorPlottingCallback(std::shared_ptr plotter); /** * @brief Plots the error @@ -65,19 +63,19 @@ class CartesianErrorPlottingCallback : public trajopt_sqp::SQPCallback * @brief addConstraintSet Adds a constraint set to be plotted * @param cart_position_cnt Constraint to be plotted */ - void addConstraintSet(const trajopt_ifopt::CartPosConstraint::ConstPtr& cart_position_cnt); + void addConstraintSet(const std::shared_ptr& cart_position_cnt); /** * @brief addConstraintSet Adds a vector of constraint sets to be plotted * @param cart_position_cnts Vector of constraints to be plotted */ - void addConstraintSet(const std::vector& cart_position_cnts); + void addConstraintSet(const std::vector>& cart_position_cnts); - bool execute(const QPProblem& problem, const trajopt_sqp::SQPResults& sqp_results) override; + bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - std::vector cart_position_cnts_; - tesseract_visualization::Visualization::Ptr plotter_; + std::vector> cart_position_cnts_; + std::shared_ptr plotter_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/clear_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/clear_plotter.h index 010f9c92..b2c9aa57 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/clear_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/clear_plotter.h @@ -29,12 +29,10 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include namespace trajopt_sqp { @@ -42,18 +40,18 @@ namespace trajopt_sqp * @brief Clears the plotter passed in. Add this callback before plotting callbacks to clear the plotter before plotting * new results */ -class ClearPlotterCallback : public trajopt_sqp::SQPCallback +class ClearPlotterCallback : public SQPCallback { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - ClearPlotterCallback(tesseract_visualization::Visualization::Ptr plotter); + ClearPlotterCallback(std::shared_ptr plotter); - bool execute(const QPProblem& problem, const trajopt_sqp::SQPResults& sqp_results) override; + bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - tesseract_visualization::Visualization::Ptr plotter_; + std::shared_ptr plotter_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h index 9208efc0..63ecabe8 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/collision_plotter.h @@ -28,36 +28,35 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP -#include #include -#include -#include namespace trajopt_sqp { -class CollisionPlottingCallback : public trajopt_sqp::SQPCallback +class CollisionPlottingCallback : public SQPCallback { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - CollisionPlottingCallback(tesseract_visualization::Visualization::Ptr plotter); + CollisionPlottingCallback(std::shared_ptr plotter); void plot(const QPProblem& problem); - void addConstraintSet(const trajopt_ifopt::DiscreteCollisionConstraint::ConstPtr& collision_constraint); + void addConstraintSet(const std::shared_ptr& collision_constraint); - void addConstraintSet(const std::vector& collision_constraints); + void addConstraintSet( + const std::vector>& collision_constraints); - bool execute(const QPProblem& problem, const trajopt_sqp::SQPResults& sqp_results) override; + bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - std::vector collision_constraints_; - tesseract_visualization::Visualization::Ptr plotter_; + std::vector> collision_constraints_; + std::shared_ptr plotter_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h index d05e854c..c5b7804d 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h @@ -28,21 +28,20 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include -#include namespace trajopt_sqp { /** * @brief SQPCallback that plots joint positions as a Tesseract Trajectory */ -class JointStatePlottingCallback : public trajopt_sqp::SQPCallback +class JointStatePlottingCallback : public SQPCallback { public: using Ptr = std::shared_ptr; @@ -52,8 +51,8 @@ class JointStatePlottingCallback : public trajopt_sqp::SQPCallback * @brief Constructor for the callback * @param plotter Plotter used to plot the joint state */ - JointStatePlottingCallback(tesseract_visualization::Visualization::Ptr plotter, - tesseract_scene_graph::StateSolver::UPtr state_solver); + JointStatePlottingCallback(std::shared_ptr plotter, + std::unique_ptr state_solver); /** * @brief Plot the joint_position variables as a tesseract trajectory @@ -65,20 +64,20 @@ class JointStatePlottingCallback : public trajopt_sqp::SQPCallback * @brief Add a variable set to be plotted * @param joint_position JointPosition variable to be plotted. They should all be the same size */ - void addVariableSet(const trajopt_ifopt::JointPosition::ConstPtr& joint_position); + void addVariableSet(const std::shared_ptr& joint_position); /** * @brief Adds multiple variable sets to be plotted * @param joint_positions JointPosition variables to be plotted. They should all be the same size */ - void addVariableSet(const std::vector& joint_positions); + void addVariableSet(const std::vector>& joint_positions); - bool execute(const QPProblem& problem, const trajopt_sqp::SQPResults& sqp_results) override; + bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - std::vector joint_positions_; - tesseract_visualization::Visualization::Ptr plotter_; - tesseract_scene_graph::StateSolver::UPtr state_solver_; + std::vector> joint_positions_; + std::shared_ptr plotter_; + std::unique_ptr state_solver_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/wait_for_input.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/wait_for_input.h index 40150523..d9cbe3b2 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/wait_for_input.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/wait_for_input.h @@ -28,13 +28,10 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include -#include -#include namespace trajopt_sqp { @@ -42,18 +39,18 @@ namespace trajopt_sqp * @brief Calls the waitForInput function in the plotter. Use to pause the optimization while displaying intermediate * results */ -class WaitForInputCallback : public trajopt_sqp::SQPCallback +class WaitForInputCallback : public SQPCallback { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - WaitForInputCallback(tesseract_visualization::Visualization::Ptr plotter); + WaitForInputCallback(std::shared_ptr plotter); - bool execute(const QPProblem& problem, const trajopt_sqp::SQPResults& sqp_results) override; + bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - tesseract_visualization::Visualization::Ptr plotter_; + std::shared_ptr plotter_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/eigen_types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/eigen_types.h new file mode 100644 index 00000000..a3ea0db5 --- /dev/null +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/eigen_types.h @@ -0,0 +1,37 @@ +/** + * @file eigen_types.h + * @brief Contains eigen types for the trust region sqp solver + * + * @author Matthew Powelson + * @date May 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TRAJOPT_SQP_EIGEN_TYPES_H +#define TRAJOPT_SQP_EIGEN_TYPES_H + +#include + +namespace trajopt_sqp +{ +using SparseMatrix = Eigen::SparseMatrix; +using SparseVector = Eigen::SparseVector; +} // namespace trajopt_sqp + +#endif // TRAJOPT_SQP_EIGEN_TYPES_H diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h index 6e3abedb..f76c94b7 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/expressions.h @@ -1,8 +1,9 @@ #ifndef TRAJOPT_SQP_EXPRESSIONS_H #define TRAJOPT_SQP_EXPRESSIONS_H -#include -#include +#include + +#include namespace trajopt_sqp { diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h new file mode 100644 index 00000000..6f5b5240 --- /dev/null +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/fwd.h @@ -0,0 +1,44 @@ +#ifndef TRAJOPT_SQP_FWD_H +#define TRAJOPT_SQP_FWD_H + +namespace trajopt_sqp +{ +// types.h +enum class ConstraintType; +enum class CostPenaltyType; +enum class SQPStatus; +struct SQPParameters; +struct SQPResults; + +// expressions.h +struct Exprs; +struct AffExprs; +struct QuadExprs; + +// qp_problem.h +class QPProblem; + +// qp_solver.h +enum class QPSolverStatus; +class QPSolver; + +// osqp_eigen_solver.h +class OSQPEigenSolver; + +// ifopt_qp_problem.h +class IfoptQPProblem; + +// trajopt_qp_problem.h +class TrajOptQPProblem; + +// callbacks +class SQPCallback; +class CartesianErrorPlottingCallback; +class ClearPlotterCallback; +class CollisionPlottingCallback; +class JointStatePlottingCallback; +class WaitForInputCallback; + +} // namespace trajopt_sqp + +#endif // TRAJOPT_SQP_FWD_H diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h index 361a084f..f674aeaa 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/ifopt_qp_problem.h @@ -28,9 +28,13 @@ #include #include -#include #include +namespace ifopt +{ +class Problem; +} + namespace trajopt_sqp { /** @brief Converts a general NLP into a convexified QP that can be solved by a QP solver */ @@ -43,11 +47,11 @@ class IfoptQPProblem : public QPProblem IfoptQPProblem(); IfoptQPProblem(std::shared_ptr nlp); - void addVariableSet(ifopt::VariableSet::Ptr variable_set) override; + void addVariableSet(std::shared_ptr variable_set) override; - void addConstraintSet(ifopt::ConstraintSet::Ptr constraint_set) override; + void addConstraintSet(std::shared_ptr constraint_set) override; - void addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPenaltyType penalty_type) override; + void addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type) override; void setup() override; diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h index aa68c9d2..31256a1e 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h @@ -26,9 +26,12 @@ #ifndef TRAJOPT_SQP_INCLUDE_OSQP_EIGEN_SOLVER_H_ #define TRAJOPT_SQP_INCLUDE_OSQP_EIGEN_SOLVER_H_ -#include #include -#include + +namespace OsqpEigen +{ +class Solver; +} namespace trajopt_sqp { @@ -64,9 +67,9 @@ class OSQPEigenSolver : public QPSolver bool updateLinearConstraintsMatrix(const SparseMatrix& linearConstraintsMatrix) override; - QPSolverStatus getSolverStatus() const override { return solver_status_; }; + QPSolverStatus getSolverStatus() const override { return solver_status_; } - OsqpEigen::Solver solver_; + std::unique_ptr solver_; private: // Depending on what they decide to do with this issue, these could be dropped diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h index ae964a81..bef3f1f6 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h @@ -2,12 +2,18 @@ #define TRAJOPT_SQP_QP_PROBLEM_BASE_H #include -#include -#include -#include +#include + +namespace ifopt +{ +class VariableSet; +class ConstraintSet; +} // namespace ifopt namespace trajopt_sqp { +enum class CostPenaltyType; + /** @brief QP Problem Base */ class QPProblem { @@ -25,7 +31,7 @@ class QPProblem * the optimal timing values. This function correctly appends the * individual variables sets and ensures correct order of Jacobian columns. */ - virtual void addVariableSet(ifopt::VariableSet::Ptr variable_set) = 0; + virtual void addVariableSet(std::shared_ptr variable_set) = 0; /** * @brief Add a set of multiple constraints to the optimization problem. @@ -35,7 +41,7 @@ class QPProblem * constraints. It makes sure the overall constraint and Jacobian correctly * considers all individual constraint sets. */ - virtual void addConstraintSet(ifopt::ConstraintSet::Ptr constraint_set) = 0; + virtual void addConstraintSet(std::shared_ptr constraint_set) = 0; /** * @brief Add a squared cost term to the problem. @@ -45,7 +51,7 @@ class QPProblem * composed of different cost terms. It makes sure the overall value and * gradient is considering each individual cost. */ - virtual void addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPenaltyType penalty_type) = 0; + virtual void addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type) = 0; /** * @brief This setups the QP problems based on the constraints and cost sets added to the problem. diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h index 0893bb8e..60625865 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h @@ -27,7 +27,8 @@ #ifndef TRAJOPT_SQP_INCLUDE_QP_SOLVER_H_ #define TRAJOPT_SQP_INCLUDE_QP_SOLVER_H_ -#include +#include +#include namespace trajopt_sqp { diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/sqp_callback.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/sqp_callback.h index 6702bdd2..ae09617b 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/sqp_callback.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/sqp_callback.h @@ -26,11 +26,12 @@ #ifndef TRAJOPT_SQP_INCLUDE_SQP_CALLBACK_H_ #define TRAJOPT_SQP_INCLUDE_SQP_CALLBACK_H_ -#include -#include +#include namespace trajopt_sqp { +class QPProblem; +struct SQPResults; /** * @brief Base class for callbacks called during the SQP routine */ diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h index 2e936475..53d5a6c0 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trajopt_qp_problem.h @@ -1,10 +1,8 @@ #ifndef TRAJOPT_SQP_TRAJOPT_QP_PROBLEM_H #define TRAJOPT_SQP_TRAJOPT_QP_PROBLEM_H +#include #include -#include -#include -#include namespace trajopt_sqp { @@ -16,12 +14,13 @@ class TrajOptQPProblem : public QPProblem using ConstPtr = std::shared_ptr; TrajOptQPProblem(); + ~TrajOptQPProblem(); - void addVariableSet(ifopt::VariableSet::Ptr variable_set) override; + void addVariableSet(std::shared_ptr variable_set) override; - void addConstraintSet(ifopt::ConstraintSet::Ptr constraint_set) override; + void addConstraintSet(std::shared_ptr constraint_set) override; - void addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPenaltyType penalty_type) override; + void addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type) override; void setup() override; @@ -77,98 +76,8 @@ class TrajOptQPProblem : public QPProblem Eigen::Ref getBoundsUpper() override; protected: - bool initialized_{ false }; - ifopt::Composite::Ptr variables_; - ifopt::Composite constraints_; - ifopt::Composite squared_costs_; - ifopt::Composite hinge_costs_; - ifopt::Composite hinge_constraints_; - ifopt::Composite abs_costs_; - ifopt::Composite abs_constraints_; - Eigen::VectorXd squared_costs_target_; - - std::vector constraint_types_; - - Eigen::Index num_qp_vars_{ 0 }; - Eigen::Index num_qp_cnts_{ 0 }; - - std::vector constraint_names_; - std::vector cost_names_; - - /** @brief Box size - constraint is set at current_val +/- box_size */ - Eigen::VectorXd box_size_; - Eigen::VectorXd constraint_merit_coeff_; - - SparseMatrix hessian_; - Eigen::VectorXd gradient_; - QuadExprs squared_objective_nlp_; - - SparseMatrix constraint_matrix_; - Eigen::VectorXd bounds_lower_; - Eigen::VectorXd bounds_upper_; - // This should be the center of the bounds - Eigen::VectorXd constraint_constant_; - - /** - * @brief Helper that updates the objective function constant and linear and quadratic coefficients of the QP Problem - * @details Called by convexify() - */ - void convexifyCosts(); - - /** - * @brief Helper that linearizes the constraints about the current point, storing the - * jacobian as the constraint matrix and adding slack variables. - * @details Called by convexify() - */ - void linearizeConstraints(); - - /** - * @brief Helper that updates the NLP constraint bounds (top section) - * @details Called by convexify() - */ - void updateNLPConstraintBounds(); - - /** - * @brief Helper that updates the NLP variable bounds (middle section) - * @details Called by convexify() - */ - void updateNLPVariableBounds(); - - /** - * @brief Called by convexify() - helper that updates the slack variable bounds (bottom section) - * @details A slack variable is referred to as an additional variable that has been introduced - * to the optimization problem to turn a inequality constraint into an equality constraint. - * Information taken from: https://en.wikipedia.org/wiki/Slack_variable - * - * As with the other variables in the augmented constraints, the slack variable cannot take on - * negative values, as the simplex algorithm requires them to be positive or zero. - * - * - If a slack variable associated with a constraint is zero at a particular candidate solution, - * the constraint is binding there, as the constraint restricts the possible changes from that point. - * - If a slack variable is positive at a particular candidate solution, the constraint is non-binding - * there, as the constraint does not restrict the possible changes from that point. - * - If a slack variable is negative at some point, the point is infeasible (not allowed), as it does - * not satisfy the constraint. - * - * Terminology - * - * - If an inequality constraint holds with equality at the optimal point, the constraint is said to - * be binding, as the point cannot be varied in the direction of the constraint even though doing - * so would improve the value of the objective function. - * - If an inequality constraint holds as a strict inequality at the optimal point (that is, does - * not hold with equality), the constraint is said to be non-binding, as the point could be varied - * in the direction of the constraint, although it would not be optimal to do so. Under certain - * conditions, as for example in convex optimization, if a constraint is non-binding, the optimization - * problem would have the same solution even in the absence of that constraint. - * - If a constraint is not satisfied at a given point, the point is said to be infeasible. - * - * @example By introducing the slack variable y >= 0, the inequality Ax <= b can be converted - * to the equation Ax + y = b. - */ - void updateSlackVariableBounds(); - - /** @brief This calculates the constant expression in the quadratic expression for the constraints */ - void updateConstraintsConstantExpression(); + struct Implementation; + std::unique_ptr impl_; }; } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h index 3af13eb3..f687adb1 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h @@ -31,9 +31,11 @@ #ifndef TRAJOPT_SQP_INCLUDE_SIMPLE_SQP_SOLVER_H_ #define TRAJOPT_SQP_INCLUDE_SIMPLE_SQP_SOLVER_H_ -#include -#include -#include +#include +#include + +#include +#include namespace trajopt_sqp { @@ -46,11 +48,11 @@ class TrustRegionSQPSolver using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrustRegionSQPSolver(QPSolver::Ptr qp_solver); + TrustRegionSQPSolver(std::shared_ptr qp_solver); - bool init(QPProblem::Ptr qp_prob); + bool init(std::shared_ptr qp_prob); - void solve(const QPProblem::Ptr& qp_prob); + void solve(const std::shared_ptr& qp_prob); /** * @brief Run a single convexification step which calls runTrustRegionLoop @@ -101,13 +103,7 @@ class TrustRegionSQPSolver void printStepInfo() const; /** @brief Registers an optimization callback */ - void registerCallback(const SQPCallback::Ptr& callback); - - /** @brief If true then debug information will be printed to the terminal */ - bool verbose{ false }; - - /** @brief Contains parameters that control the SQP optimization */ - SQPParameters params; + void registerCallback(const std::shared_ptr& callback); /** @brief Gets the optimization status (currently unset) */ const SQPStatus& getStatus(); @@ -115,16 +111,22 @@ class TrustRegionSQPSolver /** @brief Gets the SQP optimization results */ const SQPResults& getResults(); + /** @brief If true then debug information will be printed to the terminal */ + bool verbose{ false }; + + /** @brief Contains parameters that control the SQP optimization */ + SQPParameters params; + /** @brief The QP Solver used to solve a single step of the SQP routine */ - QPSolver::Ptr qp_solver; + std::shared_ptr qp_solver; /** @brief The QP problem created from the NLP */ - QPProblem::Ptr qp_problem; + std::shared_ptr qp_problem; protected: - SQPStatus status_{ SQPStatus::QP_SOLVER_ERROR }; + SQPStatus status_; SQPResults results_; - std::vector callbacks_; + std::vector> callbacks_; void constraintMeritCoeffChanged(); }; diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h index 071fd18c..a4985182 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/types.h @@ -26,14 +26,10 @@ #ifndef TRAJOPT_SQP_TYPES_H_ #define TRAJOPT_SQP_TYPES_H_ -#include -#include +#include namespace trajopt_sqp { -using SparseMatrix = Eigen::SparseMatrix; -using SparseVector = Eigen::SparseVector; - enum class ConstraintType { EQ, @@ -96,24 +92,9 @@ struct SQPResults { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - SQPResults(Eigen::Index num_vars, Eigen::Index num_cnts, Eigen::Index num_costs) - { - best_constraint_violations = Eigen::VectorXd::Zero(num_cnts); - new_constraint_violations = Eigen::VectorXd::Zero(num_cnts); - best_approx_constraint_violations = Eigen::VectorXd::Zero(num_cnts); - new_approx_constraint_violations = Eigen::VectorXd::Zero(num_cnts); - - best_costs = Eigen::VectorXd::Zero(num_costs); - new_costs = Eigen::VectorXd::Zero(num_costs); - best_approx_costs = Eigen::VectorXd::Zero(num_costs); - new_approx_costs = Eigen::VectorXd::Zero(num_costs); - - best_var_vals = Eigen::VectorXd::Zero(num_vars); - new_var_vals = Eigen::VectorXd::Zero(num_vars); - box_size = Eigen::VectorXd::Ones(num_vars); - merit_error_coeffs = Eigen::VectorXd::Ones(num_cnts); - } SQPResults() = default; + SQPResults(Eigen::Index num_vars, Eigen::Index num_cnts, Eigen::Index num_costs); + /** @brief The lowest cost ever achieved */ double best_exact_merit{ std::numeric_limits::max() }; /** @brief The cost achieved this iteration */ @@ -170,42 +151,7 @@ struct SQPResults int trust_region_iteration{ 0 }; int overall_iteration{ 0 }; - void print() const - { - Eigen::IOFormat format(3); - std::cout << "-------------- SQPResults::print() --------------" << std::endl; - std::cout << "best_exact_merit: " << best_exact_merit << std::endl; - std::cout << "new_exact_merit: " << new_exact_merit << std::endl; - std::cout << "best_approx_merit: " << best_approx_merit << std::endl; - std::cout << "new_approx_merit: " << new_approx_merit << std::endl; - - std::cout << "best_var_vals: " << best_var_vals.transpose().format(format) << std::endl; - std::cout << "new_var_vals: " << new_var_vals.transpose().format(format) << std::endl; - - std::cout << "approx_merit_improve: " << approx_merit_improve << std::endl; - std::cout << "exact_merit_improve: " << exact_merit_improve << std::endl; - std::cout << "merit_improve_ratio: " << merit_improve_ratio << std::endl; - - std::cout << "box_size: " << box_size.transpose().format(format) << std::endl; - std::cout << "merit_error_coeffs: " << merit_error_coeffs.transpose().format(format) << std::endl; - - std::cout << "best_constraint_violations: " << best_constraint_violations.transpose().format(format) << std::endl; - std::cout << "new_constraint_violations: " << new_constraint_violations.transpose().format(format) << std::endl; - std::cout << "best_approx_constraint_violations: " << best_approx_constraint_violations.transpose().format(format) - << std::endl; - std::cout << "new_approx_constraint_violations: " << new_approx_constraint_violations.transpose().format(format) - << std::endl; - - std::cout << "best_costs: " << best_costs.transpose().format(format) << std::endl; - std::cout << "new_costs: " << new_costs.transpose().format(format) << std::endl; - std::cout << "best_approx_costs: " << best_approx_costs.transpose().format(format) << std::endl; - std::cout << "new_approx_costs: " << new_approx_costs.transpose().format(format) << std::endl; - - std::cout << "penalty_iteration: " << penalty_iteration << std::endl; - std::cout << "convexify_iteration: " << convexify_iteration << std::endl; - std::cout << "trust_region_iteration: " << trust_region_iteration << std::endl; - std::cout << "overall_iteration: " << overall_iteration << std::endl; - } + void print() const; }; /** diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp index 573bc98b..a19a0f6d 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/cartesian_error_plotter.cpp @@ -24,14 +24,19 @@ * limitations under the License. */ #include -#include +#include + +#include + +#include #include #include using namespace trajopt_sqp; -CartesianErrorPlottingCallback::CartesianErrorPlottingCallback(tesseract_visualization::Visualization::Ptr plotter) +CartesianErrorPlottingCallback::CartesianErrorPlottingCallback( + std::shared_ptr plotter) : plotter_(std::move(plotter)) { } @@ -62,13 +67,13 @@ void CartesianErrorPlottingCallback::plot(const QPProblem& /*problem*/) } void CartesianErrorPlottingCallback::addConstraintSet( - const trajopt_ifopt::CartPosConstraint::ConstPtr& cart_position_cnt) + const std::shared_ptr& cart_position_cnt) { cart_position_cnts_.push_back(cart_position_cnt); -}; +} void CartesianErrorPlottingCallback::addConstraintSet( - const std::vector& cart_position_cnts) + const std::vector>& cart_position_cnts) { for (const auto& cnt : cart_position_cnts) cart_position_cnts_.push_back(cnt); @@ -78,4 +83,4 @@ bool CartesianErrorPlottingCallback::execute(const QPProblem& problem, const tra { plot(problem); return true; -}; +} diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/clear_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/clear_plotter.cpp index 6b2e9146..7f486831 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/clear_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/clear_plotter.cpp @@ -25,16 +25,16 @@ * limitations under the License. */ #include -#include +#include using namespace trajopt_sqp; -ClearPlotterCallback::ClearPlotterCallback(tesseract_visualization::Visualization::Ptr plotter) +ClearPlotterCallback::ClearPlotterCallback(std::shared_ptr plotter) : plotter_(std::move(plotter)) { } -bool ClearPlotterCallback::execute(const QPProblem& /*problem*/, const trajopt_sqp::SQPResults&) +bool ClearPlotterCallback::execute(const QPProblem& /*problem*/, const SQPResults&) { plotter_->clear(); return true; diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp index ac6ca7fa..46d60138 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp @@ -24,11 +24,15 @@ * limitations under the License. */ #include -#include +#include + +#include + +#include using namespace trajopt_sqp; -CollisionPlottingCallback::CollisionPlottingCallback(tesseract_visualization::Visualization::Ptr plotter) +CollisionPlottingCallback::CollisionPlottingCallback(std::shared_ptr plotter) : plotter_(std::move(plotter)) { } @@ -39,19 +43,19 @@ void CollisionPlottingCallback::plot(const QPProblem& /*problem*/) // NOLINT Re } void CollisionPlottingCallback::addConstraintSet( - const trajopt_ifopt::DiscreteCollisionConstraint::ConstPtr& collision_constraint) + const std::shared_ptr& collision_constraint) { collision_constraints_.push_back(collision_constraint); } void CollisionPlottingCallback::addConstraintSet( - const std::vector& collision_constraints) + const std::vector >& collision_constraints) { for (const auto& cnt : collision_constraints) collision_constraints_.push_back(cnt); } -bool CollisionPlottingCallback::execute(const QPProblem& problem, const trajopt_sqp::SQPResults&) +bool CollisionPlottingCallback::execute(const QPProblem& problem, const SQPResults&) { plot(problem); return true; diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp index 81c1fe56..42b5c334 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp @@ -24,12 +24,18 @@ * limitations under the License. */ #include + +#include #include +#include +#include +#include + using namespace trajopt_sqp; -JointStatePlottingCallback::JointStatePlottingCallback(tesseract_visualization::Visualization::Ptr plotter, - tesseract_scene_graph::StateSolver::UPtr state_solver) +JointStatePlottingCallback::JointStatePlottingCallback(std::shared_ptr plotter, + std::unique_ptr state_solver) : plotter_(std::move(plotter)), state_solver_(std::move(state_solver)) { } @@ -42,19 +48,20 @@ void JointStatePlottingCallback::plot(const QPProblem& /*problem*/) plotter_->plotTrajectory(trajectory, *state_solver_); } -void JointStatePlottingCallback::addVariableSet(const trajopt_ifopt::JointPosition::ConstPtr& joint_position) +void JointStatePlottingCallback::addVariableSet( + const std::shared_ptr& joint_position) { joint_positions_.push_back(joint_position); -}; +} void JointStatePlottingCallback::addVariableSet( - const std::vector& joint_positions) + const std::vector >& joint_positions) { for (const auto& cnt : joint_positions) joint_positions_.push_back(cnt); } -bool JointStatePlottingCallback::execute(const QPProblem& problem, const trajopt_sqp::SQPResults& /*sqp_results*/) +bool JointStatePlottingCallback::execute(const QPProblem& problem, const SQPResults& /*sqp_results*/) { plot(problem); return true; -}; +} diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/wait_for_input.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/wait_for_input.cpp index 6b385a1b..1faabf12 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/wait_for_input.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/wait_for_input.cpp @@ -24,16 +24,16 @@ * limitations under the License. */ #include -#include +#include using namespace trajopt_sqp; -WaitForInputCallback::WaitForInputCallback(tesseract_visualization::Visualization::Ptr plotter) +WaitForInputCallback::WaitForInputCallback(std::shared_ptr plotter) : plotter_(std::move(plotter)) { } -bool WaitForInputCallback::execute(const QPProblem& /*problem*/, const trajopt_sqp::SQPResults&) +bool WaitForInputCallback::execute(const QPProblem& /*problem*/, const SQPResults&) { plotter_->waitForInput(); return true; diff --git a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp index f049ddf1..bbf03426 100644 --- a/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/ifopt_qp_problem.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include namespace trajopt_sqp @@ -34,14 +35,17 @@ namespace trajopt_sqp IfoptQPProblem::IfoptQPProblem() : nlp_(std::make_shared()) {} IfoptQPProblem::IfoptQPProblem(std::shared_ptr nlp) : nlp_(std::move(nlp)) {} -void IfoptQPProblem::addVariableSet(ifopt::VariableSet::Ptr variable_set) { nlp_->AddVariableSet(variable_set); } +void IfoptQPProblem::addVariableSet(std::shared_ptr variable_set) +{ + nlp_->AddVariableSet(variable_set); +} -void IfoptQPProblem::addConstraintSet(ifopt::ConstraintSet::Ptr constraint_set) +void IfoptQPProblem::addConstraintSet(std::shared_ptr constraint_set) { nlp_->AddConstraintSet(constraint_set); } -void IfoptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPenaltyType penalty_type) +void IfoptQPProblem::addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type) { switch (penalty_type) { diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index d5709267..c7c6a006 100644 --- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp @@ -25,25 +25,26 @@ */ #include -#include #include +#include + const bool OSQP_COMPARE_DEBUG_MODE = false; namespace trajopt_sqp { -OSQPEigenSolver::OSQPEigenSolver() +OSQPEigenSolver::OSQPEigenSolver() : solver_(std::make_unique()) { if (verbosity > 0) - solver_.settings()->setVerbosity(true); + solver_->settings()->setVerbosity(true); else - solver_.settings()->setVerbosity(false); - solver_.settings()->setWarmStart(true); - solver_.settings()->setPolish(true); - solver_.settings()->setAdaptiveRho(true); - solver_.settings()->setMaxIteration(8192); - solver_.settings()->setAbsoluteTolerance(1e-4); - solver_.settings()->setRelativeTolerance(1e-6); + solver_->settings()->setVerbosity(false); + solver_->settings()->setWarmStart(true); + solver_->settings()->setPolish(true); + solver_->settings()->setAdaptiveRho(true); + solver_->settings()->setMaxIteration(8192); + solver_->settings()->setAbsoluteTolerance(1e-4); + solver_->settings()->setRelativeTolerance(1e-6); } bool OSQPEigenSolver::init(Eigen::Index num_vars, Eigen::Index num_cnts) @@ -51,8 +52,8 @@ bool OSQPEigenSolver::init(Eigen::Index num_vars, Eigen::Index num_cnts) // Set the solver size num_cnts_ = num_cnts; num_vars_ = num_vars; - solver_.data()->setNumberOfVariables(static_cast(num_vars_)); - solver_.data()->setNumberOfConstraints(static_cast(num_cnts_)); + solver_->data()->setNumberOfVariables(static_cast(num_vars_)); + solver_->data()->setNumberOfConstraints(static_cast(num_cnts_)); solver_status_ = QPSolverStatus::INITIALIZED; @@ -62,22 +63,22 @@ bool OSQPEigenSolver::init(Eigen::Index num_vars, Eigen::Index num_cnts) bool OSQPEigenSolver::clear() { // Clear all data - solver_.clearSolver(); - solver_.data()->clearHessianMatrix(); - solver_.data()->clearLinearConstraintsMatrix(); + solver_->clearSolver(); + solver_->data()->clearHessianMatrix(); + solver_->data()->clearLinearConstraintsMatrix(); return true; } bool OSQPEigenSolver::solve() { // In order to call initSolver, everything must have already been set, so we call it right before solving - if (!solver_.isInitialized()) // NOLINT - solver_.initSolver(); + if (!solver_->isInitialized()) // NOLINT + solver_->initSolver(); if (OSQP_COMPARE_DEBUG_MODE) { Eigen::IOFormat format(5); - const auto* osqp_data = solver_.data()->getData(); + const auto* osqp_data = solver_->data()->getData(); std::cout << "OSQP Number of Variables:" << osqp_data->n << std::endl; std::cout << "OSQP Number of Constraints:" << osqp_data->m << std::endl; @@ -116,17 +117,17 @@ bool OSQPEigenSolver::solve() } /** @todo Need to check if this is what we want in the new version */ - const bool solved = solver_.solve(); - const auto status = static_cast(solver_.workspace()->info->status_val); + const bool solved = solver_->solve(); + const auto status = static_cast(solver_->workspace()->info->status_val); if (OSQP_COMPARE_DEBUG_MODE) - std::cout << "OSQP Status Value: " << solver_.workspace()->info->status_val << std::endl; + std::cout << "OSQP Status Value: " << solver_->workspace()->info->status_val << std::endl; if (solved || status == OSQP_SOLVED_INACCURATE) { if (OSQP_COMPARE_DEBUG_MODE) { Eigen::IOFormat format(5); - std::cout << "OSQP Solution: " << solver_.getSolution().transpose().format(format) << std::endl; + std::cout << "OSQP Solution: " << solver_->getSolution().transpose().format(format) << std::endl; } return true; } @@ -134,10 +135,10 @@ bool OSQPEigenSolver::solve() if (verbosity > 0) // NOLINT { // If primal infeasible - if (solver_.workspace()->info->status_val == -3) + if (solver_->workspace()->info->status_val == -3) { - Eigen::Map primal_certificate(solver_.workspace()->delta_x, num_cnts_, 1); - std::cout << "OSQP Status: " << solver_.workspace()->info->status << std::endl; + Eigen::Map primal_certificate(solver_->workspace()->delta_x, num_cnts_, 1); + std::cout << "OSQP Status: " << solver_->workspace()->info->status << std::endl; std::cout << "\n---------------------------------------\n"; std::cout << std::scientific << "Primal Certificate (v): " << primal_certificate.transpose() << std::endl; @@ -154,10 +155,10 @@ bool OSQPEigenSolver::solve() } // If dual infeasible - if (solver_.workspace()->info->status_val == -4) // NOLINT + if (solver_->workspace()->info->status_val == -4) // NOLINT { - Eigen::Map dual_certificate(solver_.workspace()->delta_y, num_vars_, 1); - std::cout << "OSQP Status: " << solver_.workspace()->info->status << std::endl; + Eigen::Map dual_certificate(solver_->workspace()->delta_y, num_vars_, 1); + std::cout << "OSQP Status: " << solver_->workspace()->info->status << std::endl; std::cout << "\n---------------------------------------\n"; std::cout << "Dual Certificate (x): " << dual_certificate.transpose() << std::endl; // NOLINT @@ -176,7 +177,7 @@ bool OSQPEigenSolver::solve() Eigen::VectorXd OSQPEigenSolver::getSolution() { - Eigen::VectorXd solution = solver_.getSolution(); + Eigen::VectorXd solution = solver_->getSolution(); return solution; } @@ -239,25 +240,25 @@ bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) // Also multiply by 2 because OSQP is multiplying by (1/2) for the objective fuction SparseMatrix cleaned = 2.0 * hessian.pruned(1e-7, 1); // Any value < 1e-7 will be removed - if (solver_.isInitialized()) + if (solver_->isInitialized()) { - bool success = solver_.updateHessianMatrix(cleaned); + bool success = solver_->updateHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree_fix(solver_.data()->getData()->P); - solver_.data()->getData()->P = nullptr; - solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); + csc_spfree_fix(solver_->data()->getData()->P); + solver_->data()->getData()->P = nullptr; + solver_->data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); } return success; } - solver_.data()->clearHessianMatrix(); - bool success = solver_.data()->setHessianMatrix(cleaned); + solver_->data()->clearHessianMatrix(); + bool success = solver_->data()->setHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree_fix(solver_.data()->getData()->P); - solver_.data()->getData()->P = nullptr; - solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); + csc_spfree_fix(solver_->data()->getData()->P); + solver_->data()->getData()->P = nullptr; + solver_->data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); } return success; @@ -267,22 +268,22 @@ bool OSQPEigenSolver::updateGradient(const Eigen::Ref& gr { gradient_ = gradient; - if (solver_.isInitialized()) - return solver_.updateGradient(gradient_); + if (solver_->isInitialized()) + return solver_->updateGradient(gradient_); - return solver_.data()->setGradient(gradient_); + return solver_->data()->setGradient(gradient_); } bool OSQPEigenSolver::updateLowerBound(const Eigen::Ref& lowerBound) { bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -OSQP_INFTY); - return solver_.updateLowerBound(bounds_lower_); + return solver_->updateLowerBound(bounds_lower_); } bool OSQPEigenSolver::updateUpperBound(const Eigen::Ref& upperBound) { bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); - return solver_.updateUpperBound(bounds_upper_); + return solver_->updateUpperBound(bounds_upper_); } bool OSQPEigenSolver::updateBounds(const Eigen::Ref& lowerBound, @@ -291,11 +292,11 @@ bool OSQPEigenSolver::updateBounds(const Eigen::Ref& lowe bounds_lower_ = lowerBound.cwiseMax(Eigen::VectorXd::Ones(num_cnts_) * -OSQP_INFTY); bounds_upper_ = upperBound.cwiseMin(Eigen::VectorXd::Ones(num_cnts_) * OSQP_INFTY); - if (solver_.isInitialized()) - return solver_.updateBounds(bounds_lower_, bounds_upper_); + if (solver_->isInitialized()) + return solver_->updateBounds(bounds_lower_, bounds_upper_); - bool success = solver_.data()->setLowerBound(bounds_lower_); - success &= solver_.data()->setUpperBound(bounds_upper_); + bool success = solver_->data()->setLowerBound(bounds_lower_); + success &= solver_->data()->setUpperBound(bounds_upper_); return success; } @@ -304,27 +305,27 @@ bool OSQPEigenSolver::updateLinearConstraintsMatrix(const SparseMatrix& linearCo assert(num_cnts_ == linearConstraintsMatrix.rows()); assert(num_vars_ == linearConstraintsMatrix.cols()); - solver_.data()->clearLinearConstraintsMatrix(); + solver_->data()->clearLinearConstraintsMatrix(); SparseMatrix cleaned = linearConstraintsMatrix.pruned(1e-7, 1); // Any value < 1e-7 will be removed - if (solver_.isInitialized()) + if (solver_->isInitialized()) { - bool success = solver_.updateLinearConstraintsMatrix(cleaned); + bool success = solver_->updateLinearConstraintsMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree_fix(solver_.data()->getData()->A); - solver_.data()->getData()->A = nullptr; - solver_.data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); + csc_spfree_fix(solver_->data()->getData()->A); + solver_->data()->getData()->A = nullptr; + solver_->data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); } return success; } - bool success = solver_.data()->setLinearConstraintsMatrix(cleaned); + bool success = solver_->data()->setLinearConstraintsMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree_fix(solver_.data()->getData()->A); - solver_.data()->getData()->A = nullptr; - solver_.data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); + csc_spfree_fix(solver_->data()->getData()->A); + solver_->data()->getData()->A = nullptr; + solver_->data()->getData()->A = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); } return success; } diff --git a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp index 7cb90124..c7d3f856 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trajopt_qp_problem.cpp @@ -1,6 +1,12 @@ +#include +#include +#include + #include #include +#include + #include #include @@ -30,31 +36,164 @@ namespace trajopt_sqp { -TrajOptQPProblem::TrajOptQPProblem() - : constraints_("constraint-sets", false) - , squared_costs_("squared-cost-terms", false) - , hinge_costs_("hinge-cost-terms", false) - , hinge_constraints_("hinge-constraint-sets", false) - , abs_costs_("abs-cost-terms", false) - , abs_constraints_("abs-constraint-sets", false) +struct TrajOptQPProblem::Implementation { - variables_ = std::make_shared("variable-sets", false); -} + Implementation() + : constraints_("constraint-sets", false) + , squared_costs_("squared-cost-terms", false) + , hinge_costs_("hinge-cost-terms", false) + , hinge_constraints_("hinge-constraint-sets", false) + , abs_costs_("abs-cost-terms", false) + , abs_constraints_("abs-constraint-sets", false) + { + variables_ = std::make_shared("variable-sets", false); + } -void TrajOptQPProblem::addVariableSet(ifopt::VariableSet::Ptr variable_set) + bool initialized_{ false }; + ifopt::Composite::Ptr variables_; + ifopt::Composite constraints_; + ifopt::Composite squared_costs_; + ifopt::Composite hinge_costs_; + ifopt::Composite hinge_constraints_; + ifopt::Composite abs_costs_; + ifopt::Composite abs_constraints_; + Eigen::VectorXd squared_costs_target_; + + std::vector constraint_types_; + + Eigen::Index num_qp_vars_{ 0 }; + Eigen::Index num_qp_cnts_{ 0 }; + + std::vector constraint_names_; + std::vector cost_names_; + + /** @brief Box size - constraint is set at current_val +/- box_size */ + Eigen::VectorXd box_size_; + Eigen::VectorXd constraint_merit_coeff_; + + SparseMatrix hessian_; + Eigen::VectorXd gradient_; + QuadExprs squared_objective_nlp_; + + SparseMatrix constraint_matrix_; + Eigen::VectorXd bounds_lower_; + Eigen::VectorXd bounds_upper_; + // This should be the center of the bounds + Eigen::VectorXd constraint_constant_; + + void addVariableSet(std::shared_ptr variable_set); + + void addConstraintSet(std::shared_ptr constraint_set); + + void addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type); + + void setup(); + + void setVariables(const double* x); + + Eigen::VectorXd getVariableValues() const; + + void convexify(); + + Eigen::VectorXd evaluateConvexCosts(const Eigen::Ref& var_vals); + + double evaluateTotalExactCost(const Eigen::Ref& var_vals); + + Eigen::VectorXd evaluateExactCosts(const Eigen::Ref& var_vals); + + Eigen::VectorXd evaluateConvexConstraintViolations(const Eigen::Ref& var_vals); + + Eigen::VectorXd evaluateExactConstraintViolations(const Eigen::Ref& var_vals); + + void scaleBoxSize(double& scale); + + void setBoxSize(const Eigen::Ref& box_size); + + void setConstraintMeritCoeff(const Eigen::Ref& merit_coeff); + + void print() const; + + Eigen::Index getNumNLPVars() const; + Eigen::Index getNumNLPConstraints() const; + Eigen::Index getNumNLPCosts() const; + + /** + * @brief Helper that updates the objective function constant and linear and quadratic coefficients of the QP Problem + * @details Called by convexify() + */ + void convexifyCosts(); + + /** + * @brief Helper that linearizes the constraints about the current point, storing the + * jacobian as the constraint matrix and adding slack variables. + * @details Called by convexify() + */ + void linearizeConstraints(); + + /** + * @brief Helper that updates the NLP constraint bounds (top section) + * @details Called by convexify() + */ + void updateNLPConstraintBounds(); + + /** + * @brief Helper that updates the NLP variable bounds (middle section) + * @details Called by convexify() + */ + void updateNLPVariableBounds(); + + /** + * @brief Called by convexify() - helper that updates the slack variable bounds (bottom section) + * @details A slack variable is referred to as an additional variable that has been introduced + * to the optimization problem to turn a inequality constraint into an equality constraint. + * Information taken from: https://en.wikipedia.org/wiki/Slack_variable + * + * As with the other variables in the augmented constraints, the slack variable cannot take on + * negative values, as the simplex algorithm requires them to be positive or zero. + * + * - If a slack variable associated with a constraint is zero at a particular candidate solution, + * the constraint is binding there, as the constraint restricts the possible changes from that point. + * - If a slack variable is positive at a particular candidate solution, the constraint is non-binding + * there, as the constraint does not restrict the possible changes from that point. + * - If a slack variable is negative at some point, the point is infeasible (not allowed), as it does + * not satisfy the constraint. + * + * Terminology + * + * - If an inequality constraint holds with equality at the optimal point, the constraint is said to + * be binding, as the point cannot be varied in the direction of the constraint even though doing + * so would improve the value of the objective function. + * - If an inequality constraint holds as a strict inequality at the optimal point (that is, does + * not hold with equality), the constraint is said to be non-binding, as the point could be varied + * in the direction of the constraint, although it would not be optimal to do so. Under certain + * conditions, as for example in convex optimization, if a constraint is non-binding, the optimization + * problem would have the same solution even in the absence of that constraint. + * - If a constraint is not satisfied at a given point, the point is said to be infeasible. + * + * @example By introducing the slack variable y >= 0, the inequality Ax <= b can be converted + * to the equation Ax + y = b. + */ + void updateSlackVariableBounds(); + + /** @brief This calculates the constant expression in the quadratic expression for the constraints */ + void updateConstraintsConstantExpression(); +}; + +void TrajOptQPProblem::Implementation::addVariableSet(std::shared_ptr variable_set) { variables_->AddComponent(variable_set); initialized_ = false; } -void TrajOptQPProblem::addConstraintSet(ifopt::ConstraintSet::Ptr constraint_set) +void TrajOptQPProblem::Implementation::addConstraintSet(std::shared_ptr constraint_set) { constraint_set->LinkWithVariables(variables_); constraints_.AddComponent(constraint_set); initialized_ = false; } -void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, CostPenaltyType penalty_type) +void TrajOptQPProblem::Implementation::addCostSet(std::shared_ptr constraint_set, + CostPenaltyType penalty_type) { constraint_set->LinkWithVariables(variables_); std::vector cost_bounds = constraint_set->GetBounds(); @@ -102,7 +241,7 @@ void TrajOptQPProblem::addCostSet(ifopt::ConstraintSet::Ptr constraint_set, Cost initialized_ = false; } -void TrajOptQPProblem::setup() +void TrajOptQPProblem::Implementation::setup() { hinge_constraints_.ClearComponents(); abs_constraints_.ClearComponents(); @@ -203,14 +342,14 @@ void TrajOptQPProblem::setup() initialized_ = true; } -void TrajOptQPProblem::setVariables(const double* x) +void TrajOptQPProblem::Implementation::setVariables(const double* x) { variables_->SetVariables(Eigen::Map(x, variables_->GetRows())); } -Eigen::VectorXd TrajOptQPProblem::getVariableValues() const { return variables_->GetValues(); } +Eigen::VectorXd TrajOptQPProblem::Implementation::getVariableValues() const { return variables_->GetValues(); } -void TrajOptQPProblem::convexify() +void TrajOptQPProblem::Implementation::convexify() { assert(initialized_); // NOLINT @@ -230,7 +369,204 @@ void TrajOptQPProblem::convexify() updateSlackVariableBounds(); } -void TrajOptQPProblem::convexifyCosts() +Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateConvexCosts(const Eigen::Ref& var_vals) +{ + if (getNumNLPCosts() == 0) + return {}; + + Eigen::VectorXd var_block = var_vals.head(getNumNLPVars()); + Eigen::VectorXd costs = Eigen::VectorXd::Zero(getNumNLPCosts()); + if (squared_costs_.GetRows() > 0) + { + costs.head(squared_costs_.GetRows()) = squared_objective_nlp_.values(var_block); + assert(!(costs.head(squared_costs_.GetRows()).array() < -1e-8).any()); + } + + if (hinge_costs_.GetRows() > 0) + { + Eigen::VectorXd hinge_cnt_constant = constraint_constant_.topRows(hinge_costs_.GetRows()); + auto hinge_cnt_jac = constraint_matrix_.block(0, 0, hinge_constraints_.GetRows(), getNumNLPVars()); + + Eigen::VectorXd hinge_convex_value = hinge_cnt_constant + hinge_cnt_jac * var_block; + Eigen::VectorXd hinge_cost = trajopt_ifopt::calcBoundsViolations(hinge_convex_value, hinge_costs_.GetBounds()); + + costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()) = hinge_cost; + assert(!(costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()).array() < -1e-8).any()); + } + + if (abs_costs_.GetRows() > 0) + { + Eigen::VectorXd abs_cnt_constant = constraint_constant_.middleRows(hinge_costs_.GetRows(), abs_costs_.GetRows()); + auto abs_cnt_jac = constraint_matrix_.block(hinge_costs_.GetRows(), 0, abs_constraints_.GetRows(), getNumNLPVars()); + + Eigen::VectorXd abs_convex_value = abs_cnt_constant + abs_cnt_jac * var_block; + Eigen::VectorXd abs_cost = trajopt_ifopt::calcBoundsViolations(abs_convex_value, abs_costs_.GetBounds()).cwiseAbs(); + + costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()) = abs_cost; + assert(!(costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()).array() < -1e-8) + .any()); + } + return costs; +} + +double TrajOptQPProblem::Implementation::evaluateTotalExactCost(const Eigen::Ref& var_vals) +{ + if (getNumNLPCosts() == 0) // NOLINT + return 0; + + double g{ 0 }; + setVariables(var_vals.data()); + + if (squared_costs_.GetRows() > 0) + { + Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()); + assert((error.array() < 0).any() == false); + g += error.squaredNorm(); + } + + if (abs_costs_.GetRows() > 0) + { + Eigen::VectorXd error = + trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); + assert((error.array() < 0).any() == false); + g += error.sum(); + } + + if (hinge_costs_.GetRows() > 0) + { + Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); + assert((error.array() < 0).any() == false); + g += error.sum(); + } + + return g; +} + +Eigen::VectorXd TrajOptQPProblem::Implementation::evaluateExactCosts(const Eigen::Ref& var_vals) +{ + if (getNumNLPCosts() == 0) + return {}; + + setVariables(var_vals.data()); + + Eigen::VectorXd g(getNumNLPCosts()); + Eigen::Index start_index = 0; + if (squared_costs_.GetRows() > 0) + { + g.topRows(squared_costs_.GetRows()) = + trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()).array().square(); + start_index += squared_costs_.GetRows(); + } + + if (abs_costs_.GetRows() > 0) + { + g.middleRows(start_index, abs_costs_.GetRows()) = + trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); + start_index += abs_costs_.GetRows(); + } + + if (hinge_costs_.GetRows() > 0) + { + g.middleRows(start_index, hinge_costs_.GetRows()) = + trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); + } + + return g; +} + +Eigen::VectorXd +TrajOptQPProblem::Implementation::evaluateConvexConstraintViolations(const Eigen::Ref& var_vals) +{ + Eigen::Index row_index = hinge_constraints_.GetRows() + abs_costs_.GetRows(); + // NOLINTNEXTLINE + Eigen::VectorXd result_lin = constraint_matrix_.block(row_index, 0, getNumNLPConstraints(), getNumNLPVars()) * + var_vals.head(getNumNLPVars()); // NOLINT + Eigen::VectorXd constraint_value = constraint_constant_.middleRows(row_index, getNumNLPConstraints()) + result_lin; + return trajopt_ifopt::calcBoundsViolations(constraint_value, constraints_.GetBounds()); +} + +Eigen::VectorXd +TrajOptQPProblem::Implementation::evaluateExactConstraintViolations(const Eigen::Ref& var_vals) +{ + setVariables(var_vals.data()); + Eigen::VectorXd cnt_vals = constraints_.GetValues(); + return trajopt_ifopt::calcBoundsViolations(cnt_vals, constraints_.GetBounds()); +} + +void TrajOptQPProblem::Implementation::scaleBoxSize(double& scale) +{ + box_size_ = box_size_ * scale; + updateNLPVariableBounds(); +} + +void TrajOptQPProblem::Implementation::setBoxSize(const Eigen::Ref& box_size) +{ + assert(box_size.size() == getNumNLPVars()); + box_size_ = box_size; + updateNLPVariableBounds(); +} + +void TrajOptQPProblem::Implementation::setConstraintMeritCoeff(const Eigen::Ref& merit_coeff) +{ + assert(merit_coeff.size() == getNumNLPConstraints()); + constraint_merit_coeff_ = merit_coeff; +} + +void TrajOptQPProblem::Implementation::print() const +{ + Eigen::IOFormat format(3); + + std::cout << "-------------- QPProblem::print() --------------" << std::endl; + std::cout << "Num NLP Vars: " << getNumNLPVars() << std::endl; + std::cout << "Num QP Vars: " << num_qp_vars_ << std::endl; + std::cout << "Num NLP Constraints: " << num_qp_cnts_ << std::endl; + std::cout << "Detected Constraint Type: "; + for (const auto& cnt : constraint_types_) + std::cout << static_cast(cnt) << ", "; + + std::cout << std::endl; + std::cout << "Box Size: " << box_size_.transpose().format(format) << std::endl; // NOLINT + std::cout << "Constraint Merit Coeff: " << constraint_merit_coeff_.transpose().format(format) << std::endl; + + std::cout << "Hessian:\n" << hessian_.toDense().format(format) << std::endl; + std::cout << "Gradient: " << gradient_.transpose().format(format) << std::endl; + std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << std::endl; + std::cout << "Constraint Lower Bounds: " + << bounds_lower_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) + << std::endl; + std::cout << "Constraint Upper Bounds: " + << bounds_upper_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) + << std::endl; + std::cout << "Variable Lower Bounds: " + << bounds_lower_.tail(bounds_lower_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) + .transpose() + .format(format) + << std::endl; + std::cout << "Variable Upper Bounds: " + << bounds_upper_.tail(bounds_upper_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) + .transpose() + .format(format) + << std::endl; + std::cout << "All Lower Bounds: " << bounds_lower_.transpose().format(format) << std::endl; + std::cout << "All Upper Bounds: " << bounds_upper_.transpose().format(format) << std::endl; + std::cout << "NLP values: " << std::endl; + for (const auto& v_set : variables_->GetComponents()) + std::cout << v_set->GetValues().transpose().format(format) << std::endl; +} + +Eigen::Index TrajOptQPProblem::Implementation::getNumNLPVars() const { return variables_->GetRows(); } + +Eigen::Index TrajOptQPProblem::Implementation::getNumNLPConstraints() const +{ + return static_cast(constraints_.GetBounds().size()); +} + +Eigen::Index TrajOptQPProblem::Implementation::getNumNLPCosts() const +{ + return (squared_costs_.GetRows() + abs_costs_.GetRows() + hinge_costs_.GetRows()); +} + +void TrajOptQPProblem::Implementation::convexifyCosts() { //////////////////////////////////////////////////////// // Set the Hessian (empty for now) @@ -347,7 +683,7 @@ void TrajOptQPProblem::convexifyCosts() } } -void TrajOptQPProblem::linearizeConstraints() +void TrajOptQPProblem::Implementation::linearizeConstraints() { SparseMatrix nlp_cnt_jac = constraints_.GetJacobian(); SparseMatrix hinge_cnt_jac = hinge_constraints_.GetJacobian(); @@ -429,7 +765,7 @@ void TrajOptQPProblem::linearizeConstraints() constraint_matrix_.setFromTriplets(tripletList.begin(), tripletList.end()); // NOLINT } -void TrajOptQPProblem::updateConstraintsConstantExpression() +void TrajOptQPProblem::Implementation::updateConstraintsConstantExpression() { long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); if (total_num_cnt == 0) @@ -512,7 +848,7 @@ void TrajOptQPProblem::updateConstraintsConstantExpression() } } -void TrajOptQPProblem::updateNLPConstraintBounds() +void TrajOptQPProblem::Implementation::updateNLPConstraintBounds() { long total_num_cnt = (getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows()); if (total_num_cnt == 0) @@ -556,7 +892,7 @@ void TrajOptQPProblem::updateNLPConstraintBounds() bounds_upper_.topRows(total_num_cnt) = linearized_cnt_upper; } -void TrajOptQPProblem::updateNLPVariableBounds() +void TrajOptQPProblem::Implementation::updateNLPVariableBounds() { // This is eqivalent to BasicTrustRegionSQP::setTrustBoxConstraints Eigen::VectorXd x_initial = variables_->GetValues(); @@ -585,7 +921,7 @@ void TrajOptQPProblem::updateNLPVariableBounds() bounds_upper_.block(var_row_index, 0, var_bounds_upper_final.size(), 1) = var_bounds_upper_final; } -void TrajOptQPProblem::updateSlackVariableBounds() +void TrajOptQPProblem::Implementation::updateSlackVariableBounds() { Eigen::Index current_cnt_index = getNumNLPConstraints() + hinge_constraints_.GetRows() + abs_constraints_.GetRows() + getNumNLPVars(); @@ -621,226 +957,101 @@ void TrajOptQPProblem::updateSlackVariableBounds() } } -double TrajOptQPProblem::evaluateTotalConvexCost(const Eigen::Ref& var_vals) +TrajOptQPProblem::TrajOptQPProblem() : impl_(std::make_unique()) {} + +TrajOptQPProblem::~TrajOptQPProblem() = default; + +void TrajOptQPProblem::addVariableSet(std::shared_ptr variable_set) { - return evaluateConvexCosts(var_vals).sum(); + impl_->addVariableSet(std::move(variable_set)); } -Eigen::VectorXd TrajOptQPProblem::evaluateConvexCosts(const Eigen::Ref& var_vals) +void TrajOptQPProblem::addConstraintSet(std::shared_ptr constraint_set) { - if (getNumNLPCosts() == 0) - return {}; + impl_->addConstraintSet(std::move(constraint_set)); +} - Eigen::VectorXd var_block = var_vals.head(getNumNLPVars()); - Eigen::VectorXd costs = Eigen::VectorXd::Zero(getNumNLPCosts()); - if (squared_costs_.GetRows() > 0) - { - costs.head(squared_costs_.GetRows()) = squared_objective_nlp_.values(var_block); - assert(!(costs.head(squared_costs_.GetRows()).array() < -1e-8).any()); - } +void TrajOptQPProblem::addCostSet(std::shared_ptr constraint_set, CostPenaltyType penalty_type) +{ + impl_->addCostSet(std::move(constraint_set), penalty_type); +} - if (hinge_costs_.GetRows() > 0) - { - Eigen::VectorXd hinge_cnt_constant = constraint_constant_.topRows(hinge_costs_.GetRows()); - auto hinge_cnt_jac = constraint_matrix_.block(0, 0, hinge_constraints_.GetRows(), getNumNLPVars()); +void TrajOptQPProblem::setup() { impl_->setup(); } - Eigen::VectorXd hinge_convex_value = hinge_cnt_constant + hinge_cnt_jac * var_block; - Eigen::VectorXd hinge_cost = trajopt_ifopt::calcBoundsViolations(hinge_convex_value, hinge_costs_.GetBounds()); +void TrajOptQPProblem::setVariables(const double* x) { impl_->setVariables(x); } - costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()) = hinge_cost; - assert(!(costs.middleRows(squared_costs_.GetRows(), hinge_costs_.GetRows()).array() < -1e-8).any()); - } +Eigen::VectorXd TrajOptQPProblem::getVariableValues() const { return impl_->getVariableValues(); } - if (abs_costs_.GetRows() > 0) - { - Eigen::VectorXd abs_cnt_constant = constraint_constant_.middleRows(hinge_costs_.GetRows(), abs_costs_.GetRows()); - auto abs_cnt_jac = constraint_matrix_.block(hinge_costs_.GetRows(), 0, abs_constraints_.GetRows(), getNumNLPVars()); +void TrajOptQPProblem::convexify() { impl_->convexify(); } - Eigen::VectorXd abs_convex_value = abs_cnt_constant + abs_cnt_jac * var_block; - Eigen::VectorXd abs_cost = trajopt_ifopt::calcBoundsViolations(abs_convex_value, abs_costs_.GetBounds()).cwiseAbs(); +double TrajOptQPProblem::evaluateTotalConvexCost(const Eigen::Ref& var_vals) +{ + return evaluateConvexCosts(var_vals).sum(); +} - costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()) = abs_cost; - assert(!(costs.middleRows(squared_costs_.GetRows() + hinge_costs_.GetRows(), abs_costs_.GetRows()).array() < -1e-8) - .any()); - } - return costs; +Eigen::VectorXd TrajOptQPProblem::evaluateConvexCosts(const Eigen::Ref& var_vals) +{ + return impl_->evaluateConvexCosts(var_vals); } double TrajOptQPProblem::evaluateTotalExactCost(const Eigen::Ref& var_vals) { - if (getNumNLPCosts() == 0) // NOLINT - return 0; - - double g{ 0 }; - setVariables(var_vals.data()); - - if (squared_costs_.GetRows() > 0) - { - Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()); - assert((error.array() < 0).any() == false); - g += error.squaredNorm(); - } - - if (abs_costs_.GetRows() > 0) - { - Eigen::VectorXd error = - trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); - assert((error.array() < 0).any() == false); - g += error.sum(); - } - - if (hinge_costs_.GetRows() > 0) - { - Eigen::VectorXd error = trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); - assert((error.array() < 0).any() == false); - g += error.sum(); - } - - return g; + return impl_->evaluateTotalExactCost(var_vals); } Eigen::VectorXd TrajOptQPProblem::evaluateExactCosts(const Eigen::Ref& var_vals) { - if (getNumNLPCosts() == 0) - return {}; - - setVariables(var_vals.data()); - - Eigen::VectorXd g(getNumNLPCosts()); - Eigen::Index start_index = 0; - if (squared_costs_.GetRows() > 0) - { - g.topRows(squared_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(squared_costs_.GetValues(), squared_costs_.GetBounds()).array().square(); - start_index += squared_costs_.GetRows(); - } - - if (abs_costs_.GetRows() > 0) - { - g.middleRows(start_index, abs_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(abs_costs_.GetValues(), abs_costs_.GetBounds()).cwiseAbs(); - start_index += abs_costs_.GetRows(); - } - - if (hinge_costs_.GetRows() > 0) - { - g.middleRows(start_index, hinge_costs_.GetRows()) = - trajopt_ifopt::calcBoundsViolations(hinge_costs_.GetValues(), hinge_costs_.GetBounds()); - } - - return g; + return impl_->evaluateExactCosts(var_vals); } -Eigen::VectorXd TrajOptQPProblem::getExactCosts() { return evaluateExactCosts(variables_->GetValues()); } +Eigen::VectorXd TrajOptQPProblem::getExactCosts() { return evaluateExactCosts(impl_->variables_->GetValues()); } Eigen::VectorXd TrajOptQPProblem::evaluateConvexConstraintViolations(const Eigen::Ref& var_vals) { - Eigen::Index row_index = hinge_constraints_.GetRows() + abs_costs_.GetRows(); - // NOLINTNEXTLINE - Eigen::VectorXd result_lin = constraint_matrix_.block(row_index, 0, getNumNLPConstraints(), getNumNLPVars()) * - var_vals.head(getNumNLPVars()); // NOLINT - Eigen::VectorXd constraint_value = constraint_constant_.middleRows(row_index, getNumNLPConstraints()) + result_lin; - return trajopt_ifopt::calcBoundsViolations(constraint_value, constraints_.GetBounds()); + return impl_->evaluateConvexConstraintViolations(var_vals); } Eigen::VectorXd TrajOptQPProblem::evaluateExactConstraintViolations(const Eigen::Ref& var_vals) { - setVariables(var_vals.data()); - Eigen::VectorXd cnt_vals = constraints_.GetValues(); - return trajopt_ifopt::calcBoundsViolations(cnt_vals, constraints_.GetBounds()); + return impl_->evaluateExactConstraintViolations(var_vals); } Eigen::VectorXd TrajOptQPProblem::getExactConstraintViolations() { - return evaluateExactConstraintViolations(variables_->GetValues()); // NOLINT + return evaluateExactConstraintViolations(impl_->variables_->GetValues()); // NOLINT } -void TrajOptQPProblem::scaleBoxSize(double& scale) -{ - box_size_ = box_size_ * scale; - updateNLPVariableBounds(); -} +void TrajOptQPProblem::scaleBoxSize(double& scale) { impl_->scaleBoxSize(scale); } -void TrajOptQPProblem::setBoxSize(const Eigen::Ref& box_size) -{ - assert(box_size.size() == getNumNLPVars()); - box_size_ = box_size; - updateNLPVariableBounds(); -} +void TrajOptQPProblem::setBoxSize(const Eigen::Ref& box_size) { impl_->setBoxSize(box_size); } void TrajOptQPProblem::setConstraintMeritCoeff(const Eigen::Ref& merit_coeff) { - assert(merit_coeff.size() == getNumNLPConstraints()); - constraint_merit_coeff_ = merit_coeff; + impl_->setConstraintMeritCoeff(merit_coeff); } -Eigen::VectorXd TrajOptQPProblem::getBoxSize() const { return box_size_; } +Eigen::VectorXd TrajOptQPProblem::getBoxSize() const { return impl_->box_size_; } -void TrajOptQPProblem::print() const -{ - Eigen::IOFormat format(3); - - std::cout << "-------------- QPProblem::print() --------------" << std::endl; - std::cout << "Num NLP Vars: " << getNumNLPVars() << std::endl; - std::cout << "Num QP Vars: " << num_qp_vars_ << std::endl; - std::cout << "Num NLP Constraints: " << num_qp_cnts_ << std::endl; - std::cout << "Detected Constraint Type: "; - for (const auto& cnt : constraint_types_) - std::cout << static_cast(cnt) << ", "; - - std::cout << std::endl; - std::cout << "Box Size: " << box_size_.transpose().format(format) << std::endl; // NOLINT - std::cout << "Constraint Merit Coeff: " << constraint_merit_coeff_.transpose().format(format) << std::endl; +void TrajOptQPProblem::print() const { impl_->print(); } - std::cout << "Hessian:\n" << hessian_.toDense().format(format) << std::endl; - std::cout << "Gradient: " << gradient_.transpose().format(format) << std::endl; - std::cout << "Constraint Matrix:\n" << constraint_matrix_.toDense().format(format) << std::endl; - std::cout << "Constraint Lower Bounds: " - << bounds_lower_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) - << std::endl; - std::cout << "Constraint Upper Bounds: " - << bounds_upper_.head(getNumNLPConstraints() + hinge_constraints_.GetRows()).transpose().format(format) - << std::endl; - std::cout << "Variable Lower Bounds: " - << bounds_lower_.tail(bounds_lower_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) - .transpose() - .format(format) - << std::endl; - std::cout << "Variable Upper Bounds: " - << bounds_upper_.tail(bounds_upper_.rows() - getNumNLPConstraints() - hinge_constraints_.GetRows()) - .transpose() - .format(format) - << std::endl; - std::cout << "All Lower Bounds: " << bounds_lower_.transpose().format(format) << std::endl; - std::cout << "All Upper Bounds: " << bounds_upper_.transpose().format(format) << std::endl; - std::cout << "NLP values: " << std::endl; - for (const auto& v_set : variables_->GetComponents()) - std::cout << v_set->GetValues().transpose().format(format) << std::endl; -} +Eigen::Index TrajOptQPProblem::getNumNLPVars() const { return impl_->getNumNLPVars(); } +Eigen::Index TrajOptQPProblem::getNumNLPConstraints() const { return impl_->getNumNLPConstraints(); } +Eigen::Index TrajOptQPProblem::getNumNLPCosts() const { return impl_->getNumNLPCosts(); } -Eigen::Index TrajOptQPProblem::getNumNLPVars() const { return variables_->GetRows(); } -Eigen::Index TrajOptQPProblem::getNumNLPConstraints() const -{ - return static_cast(constraints_.GetBounds().size()); -} -Eigen::Index TrajOptQPProblem::getNumNLPCosts() const -{ - return (squared_costs_.GetRows() + abs_costs_.GetRows() + hinge_costs_.GetRows()); -} -Eigen::Index TrajOptQPProblem::getNumQPVars() const { return num_qp_vars_; } -Eigen::Index TrajOptQPProblem::getNumQPConstraints() const { return num_qp_cnts_; } +Eigen::Index TrajOptQPProblem::getNumQPVars() const { return impl_->num_qp_vars_; } +Eigen::Index TrajOptQPProblem::getNumQPConstraints() const { return impl_->num_qp_cnts_; } -const std::vector& TrajOptQPProblem::getNLPConstraintNames() const { return constraint_names_; } -const std::vector& TrajOptQPProblem::getNLPCostNames() const { return cost_names_; } +const std::vector& TrajOptQPProblem::getNLPConstraintNames() const { return impl_->constraint_names_; } +const std::vector& TrajOptQPProblem::getNLPCostNames() const { return impl_->cost_names_; } -Eigen::Ref TrajOptQPProblem::getBoxSize() { return box_size_; } -Eigen::Ref TrajOptQPProblem::getConstraintMeritCoeff() { return constraint_merit_coeff_; } +Eigen::Ref TrajOptQPProblem::getBoxSize() { return impl_->box_size_; } +Eigen::Ref TrajOptQPProblem::getConstraintMeritCoeff() { return impl_->constraint_merit_coeff_; } -Eigen::Ref TrajOptQPProblem::getHessian() { return hessian_; } -Eigen::Ref TrajOptQPProblem::getGradient() { return gradient_; } +Eigen::Ref TrajOptQPProblem::getHessian() { return impl_->hessian_; } +Eigen::Ref TrajOptQPProblem::getGradient() { return impl_->gradient_; } -Eigen::Ref TrajOptQPProblem::getConstraintMatrix() { return constraint_matrix_; } -Eigen::Ref TrajOptQPProblem::getBoundsLower() { return bounds_lower_; } -Eigen::Ref TrajOptQPProblem::getBoundsUpper() { return bounds_upper_; } +Eigen::Ref TrajOptQPProblem::getConstraintMatrix() { return impl_->constraint_matrix_; } +Eigen::Ref TrajOptQPProblem::getBoundsLower() { return impl_->bounds_lower_; } +Eigen::Ref TrajOptQPProblem::getBoundsUpper() { return impl_->bounds_upper_; } } // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index 34ea048e..4130fa2b 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -29,6 +29,10 @@ * limitations under the License. */ #include +#include +#include +#include + #include #include @@ -36,7 +40,10 @@ namespace trajopt_sqp { const bool SUPER_DEBUG_MODE = false; -TrustRegionSQPSolver::TrustRegionSQPSolver(QPSolver::Ptr qp_solver) : qp_solver(std::move(qp_solver)) {} +TrustRegionSQPSolver::TrustRegionSQPSolver(QPSolver::Ptr qp_solver) + : qp_solver(std::move(qp_solver)), status_{ SQPStatus::QP_SOLVER_ERROR } +{ +} bool TrustRegionSQPSolver::init(QPProblem::Ptr qp_prob) { diff --git a/trajopt_optimizers/trajopt_sqp/src/types.cpp b/trajopt_optimizers/trajopt_sqp/src/types.cpp new file mode 100644 index 00000000..6ac9d8e4 --- /dev/null +++ b/trajopt_optimizers/trajopt_sqp/src/types.cpp @@ -0,0 +1,88 @@ +/** + * @file types.cpp + * @brief Contains types for the trust region sqp solver + * + * @author Matthew Powelson + * @date May 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include + +namespace trajopt_sqp +{ +SQPResults::SQPResults(Eigen::Index num_vars, Eigen::Index num_cnts, Eigen::Index num_costs) +{ + best_constraint_violations = Eigen::VectorXd::Zero(num_cnts); + new_constraint_violations = Eigen::VectorXd::Zero(num_cnts); + best_approx_constraint_violations = Eigen::VectorXd::Zero(num_cnts); + new_approx_constraint_violations = Eigen::VectorXd::Zero(num_cnts); + + best_costs = Eigen::VectorXd::Zero(num_costs); + new_costs = Eigen::VectorXd::Zero(num_costs); + best_approx_costs = Eigen::VectorXd::Zero(num_costs); + new_approx_costs = Eigen::VectorXd::Zero(num_costs); + + best_var_vals = Eigen::VectorXd::Zero(num_vars); + new_var_vals = Eigen::VectorXd::Zero(num_vars); + box_size = Eigen::VectorXd::Ones(num_vars); + merit_error_coeffs = Eigen::VectorXd::Ones(num_cnts); +} + +void SQPResults::print() const +{ + Eigen::IOFormat format(3); + std::cout << "-------------- SQPResults::print() --------------" << std::endl; + std::cout << "best_exact_merit: " << best_exact_merit << std::endl; + std::cout << "new_exact_merit: " << new_exact_merit << std::endl; + std::cout << "best_approx_merit: " << best_approx_merit << std::endl; + std::cout << "new_approx_merit: " << new_approx_merit << std::endl; + + std::cout << "best_var_vals: " << best_var_vals.transpose().format(format) << std::endl; + std::cout << "new_var_vals: " << new_var_vals.transpose().format(format) << std::endl; + + std::cout << "approx_merit_improve: " << approx_merit_improve << std::endl; + std::cout << "exact_merit_improve: " << exact_merit_improve << std::endl; + std::cout << "merit_improve_ratio: " << merit_improve_ratio << std::endl; + + std::cout << "box_size: " << box_size.transpose().format(format) << std::endl; + std::cout << "merit_error_coeffs: " << merit_error_coeffs.transpose().format(format) << std::endl; + + std::cout << "best_constraint_violations: " << best_constraint_violations.transpose().format(format) << std::endl; + std::cout << "new_constraint_violations: " << new_constraint_violations.transpose().format(format) << std::endl; + std::cout << "best_approx_constraint_violations: " << best_approx_constraint_violations.transpose().format(format) + << std::endl; + std::cout << "new_approx_constraint_violations: " << new_approx_constraint_violations.transpose().format(format) + << std::endl; + + std::cout << "best_costs: " << best_costs.transpose().format(format) << std::endl; + std::cout << "new_costs: " << new_costs.transpose().format(format) << std::endl; + std::cout << "best_approx_costs: " << best_approx_costs.transpose().format(format) << std::endl; + std::cout << "new_approx_costs: " << new_approx_costs.transpose().format(format) << std::endl; + + std::cout << "penalty_iteration: " << penalty_iteration << std::endl; + std::cout << "convexify_iteration: " << convexify_iteration << std::endl; + std::cout << "trust_region_iteration: " << trust_region_iteration << std::endl; + std::cout << "overall_iteration: " << overall_iteration << std::endl; +} + +} // namespace trajopt_sqp diff --git a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp index e39675e1..823ef320 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp @@ -30,12 +30,15 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include + #include #include -#include #include #include +#include +#include #include TRAJOPT_IGNORE_WARNINGS_POP @@ -77,13 +80,13 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, { auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(DEBUG); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(DEBUG); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // Extract necessary kinematic information tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp index 1e48ee77..48221fec 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp @@ -28,13 +28,28 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include +#include +#include +#include +#include +#include +#include +#include #include #include +#include +#include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + +#include #include #include #include @@ -182,7 +197,7 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); for (std::size_t i = 1; i < (vars.size() - 1); ++i) { auto collision_evaluator = std::make_shared( @@ -201,13 +216,13 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; @@ -303,7 +318,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { @@ -325,13 +340,13 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp index 1b023201..ac882be2 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp @@ -28,15 +28,27 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include +#include #include #include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + +#include #include #include #include @@ -86,7 +98,7 @@ class CastOctomapTest : public testing::TestWithParam octree->insertPointCloud(point_cloud, octomap::point3d(0, 0, 0)); // Next add objects that can be attached/detached to the scene - Octree::Ptr coll_octree = std::make_shared(octree, Octree::SubType::BOX); + Octree::Ptr coll_octree = std::make_shared(octree, OctreeSubType::BOX); auto vis_box = std::make_shared(1.0, 1.0, 1.0); auto visual = std::make_shared(); @@ -175,7 +187,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { @@ -197,13 +209,13 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp index 3ec3213a..d33194ef 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp @@ -28,14 +28,23 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include +#include +#include #include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + #include #include #include +#include #include #include @@ -130,7 +139,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { @@ -152,13 +161,13 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp index ee7dbfa2..10ec0dcb 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp @@ -28,12 +28,24 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include #include +#include +#include +#include +#include +#include +#include #include #include +#include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + +#include #include #include #include @@ -159,7 +171,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ true, false }; for (std::size_t i = 1; i < vars.size(); ++i) { @@ -181,13 +193,13 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp index e7f1f77c..18aeb24d 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp @@ -28,7 +28,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include - +#include #include #include #include @@ -62,13 +62,13 @@ void runAccelerationConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr { auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(DEBUG); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setVerbosity(DEBUG); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables std::vector vars; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp index 50e07cc6..b3e9b97c 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp @@ -29,6 +29,8 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include + #include #include #include @@ -62,13 +64,13 @@ void runJerkConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_pro { auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(DEBUG); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setVerbosity(DEBUG); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables std::vector vars; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp index 3f785180..f5af3b61 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp @@ -28,7 +28,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include - +#include #include #include #include @@ -60,13 +60,13 @@ void runJointPositionOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_prob { auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(DEBUG); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(DEBUG); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 2) Add Variables std::vector vars; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp index 0a8c2948..c2a2ed48 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp @@ -28,7 +28,7 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include - +#include #include #include #include @@ -62,13 +62,13 @@ void runVelocityConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp { auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(DEBUG); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setVerbosity(DEBUG); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables std::vector vars; diff --git a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp index e29db91c..966f8dcc 100644 --- a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp @@ -29,11 +29,18 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include +#include +#include +#include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include #include #include #include @@ -114,13 +121,13 @@ void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env // 5) Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(false); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(true); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(false); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(true); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp index 1f9f0ef1..5ea90128 100644 --- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp @@ -30,15 +30,24 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include #include +#include +#include +#include +#include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + #include #include #include #include +#include #include #include @@ -143,7 +152,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro qp_problem->addConstraintSet(cnt); } - auto collision_cache = std::make_shared(100); + auto collision_cache = std::make_shared(100); std::array position_vars_fixed{ false, false }; for (std::size_t i = 1; i < (vars.size() - 1); ++i) { @@ -171,13 +180,13 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro // Setup solver auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true; diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp index 524de451..05d2fba4 100644 --- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp @@ -28,12 +28,20 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include +#include +#include +#include #include #include #include #include TRAJOPT_IGNORE_WARNINGS_POP +#include + #include #include #include @@ -241,7 +249,7 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const auto trajopt_collision_cnt_config = std::make_shared(0.2, 1); trajopt_collision_cnt_config->collision_margin_buffer = 0.05; - auto collision_cnt_cache = std::make_shared(100); + auto collision_cnt_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cnt_evaluator = std::make_shared( collision_cnt_cache, manip, env, trajopt_collision_cnt_config); @@ -251,7 +259,7 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const auto trajopt_collision_cost_config = std::make_shared(0.3, 1); trajopt_collision_cost_config->collision_margin_buffer = 0.05; - auto collision_cost_cache = std::make_shared(100); + auto collision_cost_cache = std::make_shared(100); trajopt_ifopt::DiscreteCollisionEvaluator::Ptr collision_cost_evaluator = std::make_shared( collision_cost_cache, manip, env, trajopt_collision_cost_config); @@ -272,13 +280,13 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const // 5) choose solver and options auto qp_solver = std::make_shared(); trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - qp_solver->solver_.settings()->setVerbosity(true); - qp_solver->solver_.settings()->setWarmStart(true); - qp_solver->solver_.settings()->setPolish(true); - qp_solver->solver_.settings()->setAdaptiveRho(false); - qp_solver->solver_.settings()->setMaxIteration(8192); - qp_solver->solver_.settings()->setAbsoluteTolerance(1e-4); - qp_solver->solver_.settings()->setRelativeTolerance(1e-6); + qp_solver->solver_->settings()->setVerbosity(true); + qp_solver->solver_->settings()->setWarmStart(true); + qp_solver->solver_->settings()->setPolish(true); + qp_solver->solver_->settings()->setAdaptiveRho(false); + qp_solver->solver_->settings()->setMaxIteration(8192); + qp_solver->solver_->settings()->setAbsoluteTolerance(1e-4); + qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 6) solve solver.verbose = true;