From 66d144f37e120d0026259f717e453a7ad0262f1a Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 28 Dec 2024 09:45:56 -0600 Subject: [PATCH] New TrajOpt IFOPT profiles --- tesseract_examples/src/car_seat_example.cpp | 10 +- .../src/pick_and_place_example.cpp | 6 +- .../puzzle_piece_auxillary_axes_example.cpp | 10 +- .../src/puzzle_piece_example.cpp | 10 +- tesseract_motion_planners/CHANGELOG.rst | 2 +- .../profile/trajopt_osqp_solver_profile.h | 3 + .../trajopt_ifopt/CMakeLists.txt | 3 +- .../trajopt_ifopt/fwd.h | 2 +- .../trajopt_ifopt_default_composite_profile.h | 2 - .../trajopt_ifopt_default_plan_profile.h | 2 - .../trajopt_ifopt_default_solver_profile.h | 82 --------- .../trajopt_ifopt_osqp_solver_profile.h | 89 ++++++++++ .../profile/trajopt_ifopt_profile.h | 11 +- .../trajopt_ifopt/trajopt_ifopt_problem.h | 11 -- ...rajopt_ifopt_default_composite_profile.cpp | 6 - .../trajopt_ifopt_default_plan_profile.cpp | 6 - .../trajopt_ifopt_default_solver_profile.cpp | 82 --------- .../trajopt_ifopt_osqp_solver_profile.cpp | 155 ++++++++++++++++++ .../src/trajopt_ifopt_motion_planner.cpp | 50 ++---- .../src/trajopt_ifopt_problem.cpp | 48 ------ 20 files changed, 284 insertions(+), 306 deletions(-) delete mode 100644 tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h create mode 100644 tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h delete mode 100644 tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp create mode 100644 tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_osqp_solver_profile.cpp delete mode 100644 tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index f318817fc9..0606a9286b 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -69,7 +69,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -306,10 +306,10 @@ bool CarSeatExample::run() trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); - auto trajopt_ifopt_solver_profile = std::make_shared(); - trajopt_ifopt_solver_profile->opt_info.max_iterations = 200; - trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + auto trajopt_ifopt_solver_profile = std::make_shared(); + trajopt_ifopt_solver_profile->opt_params.max_iterations = 200; + trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3; + trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3; profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 19932ee202..2ede880431 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -47,7 +47,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -255,8 +255,8 @@ bool PickAndPlaceExample::run() trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.05; - auto trajopt_ifopt_solver_profile = std::make_shared(); - trajopt_ifopt_solver_profile->opt_info.max_iterations = 100; + auto trajopt_ifopt_solver_profile = std::make_shared(); + trajopt_ifopt_solver_profile->opt_params.max_iterations = 100; profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 7825df54b5..8d96e7e2f7 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -56,7 +56,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -251,11 +251,11 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_ifopt_composite_profile->smooth_jerks = true; trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - auto trajopt_ifopt_solver_profile = std::make_shared(); + auto trajopt_ifopt_solver_profile = std::make_shared(); // trajopt_ifopt_solver_profile->convex_solver_settings.adaptive_rho = 0; - trajopt_ifopt_solver_profile->opt_info.max_iterations = 200; - trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + trajopt_ifopt_solver_profile->opt_params.max_iterations = 200; + trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3; + trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3; profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 1f90282945..63dd94ef01 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -56,7 +56,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -242,10 +242,10 @@ bool PuzzlePieceExample::run() trajopt_ifopt_composite_profile->smooth_jerks = true; trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - auto trajopt_ifopt_solver_profile = std::make_shared(); - trajopt_ifopt_solver_profile->opt_info.max_iterations = 200; - trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + auto trajopt_ifopt_solver_profile = std::make_shared(); + trajopt_ifopt_solver_profile->opt_params.max_iterations = 200; + trajopt_ifopt_solver_profile->opt_params.min_approx_improve = 1e-3; + trajopt_ifopt_solver_profile->opt_params.min_trust_box_size = 1e-3; profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); diff --git a/tesseract_motion_planners/CHANGELOG.rst b/tesseract_motion_planners/CHANGELOG.rst index a1537c7bca..aeacfb6010 100644 --- a/tesseract_motion_planners/CHANGELOG.rst +++ b/tesseract_motion_planners/CHANGELOG.rst @@ -68,7 +68,7 @@ Changelog for package tesseract_motion_planners * More compact descartes collision logging output (`#460 `_) * Better debugging feedback on failed Descartes plan (`#401 `_) Co-authored-by: Levi Armstrong -* Add convex_solver_settings to TrajOptIfoptDefaultSolverProfile (`#425 `_) +* Add convex_solver_settings to TrajOptIfoptOSQPSolverProfile (`#425 `_) Co-authored-by: Levi Armstrong * Fix descartes planner check if the graph built * Add time parameterization interface (`#455 `_) diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h index 3bbf65ee6f..e31aa5c99c 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_osqp_solver_profile.h @@ -34,6 +34,9 @@ namespace boost::serialization template void serialize(Archive& ar, OSQPSettings& settings, const unsigned int version); // NOLINT +template +void serialize(Archive& ar, sco::BasicTrustRegionSQPParameters& params, const unsigned int version); // NOLINT + } // namespace boost::serialization namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt index 7f03805c0b..05d3fa6c79 100644 --- a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt @@ -5,12 +5,11 @@ find_package(trajopt_sqp REQUIRED) add_library( ${PROJECT_NAME}_trajopt_ifopt SHARED src/trajopt_ifopt_motion_planner.cpp - src/trajopt_ifopt_problem.cpp src/trajopt_ifopt_utils.cpp src/profile/trajopt_ifopt_profile.cpp src/profile/trajopt_ifopt_default_plan_profile.cpp src/profile/trajopt_ifopt_default_composite_profile.cpp - src/profile/trajopt_ifopt_default_solver_profile.cpp) + src/profile/trajopt_ifopt_osqp_solver_profile.cpp) target_link_libraries( ${PROJECT_NAME}_trajopt_ifopt PUBLIC ${PROJECT_NAME}_core diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/fwd.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/fwd.h index 0d5fe5e92d..de48a5d81f 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/fwd.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/fwd.h @@ -15,7 +15,7 @@ class TrajOptIfoptPlanProfile; class TrajOptIfoptCompositeProfile; class TrajOptIfoptSolverProfile; -class TrajOptIfoptDefaultSolverProfile; +class TrajOptIfoptOSQPSolverProfile; class TrajOptIfoptDefaultPlanProfile; class TrajOptIfoptDefaultCompositeProfile; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h index 2d7fb81c96..515adeef08 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h @@ -89,8 +89,6 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile const std::vector& active_links, const std::vector& fixed_indices) const override; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h index 01264d5c1a..97382c65a2 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -64,8 +64,6 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile const std::vector& active_links, int index) const override; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h deleted file mode 100644 index 04ee00a461..0000000000 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file trajopt_default_solver_profile.h - * @brief - * - * @author Levi Armstrong - * @date December 13, 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 TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace OsqpEigen -{ -class Settings; -} - -namespace tesseract_planning -{ -/** @brief The contains the default solver parameters available for setting up TrajOpt */ -class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - TrajOptIfoptDefaultSolverProfile(); - ~TrajOptIfoptDefaultSolverProfile() override; - TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete; - TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete; - TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = delete; - TrajOptIfoptDefaultSolverProfile&& operator=(TrajOptIfoptDefaultSolverProfile&&) = delete; - - /** @brief The OSQP convex solver settings to use - * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ - std::unique_ptr convex_solver_settings{ nullptr }; - - /** @brief Optimization parameters */ - trajopt_sqp::SQPParameters opt_info{}; - - /** @brief Optimization callbacks */ - std::vector> callbacks; - - void apply(TrajOptIfoptProblem& problem) const override; - - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - -protected: - friend class boost::serialization::access; - template - void serialize(Archive&, const unsigned int); // NOLINT -}; -} // namespace tesseract_planning - -BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptDefaultSolverProfile) - -#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h new file mode 100644 index 0000000000..e68b7a7bbb --- /dev/null +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_osqp_solver_profile.h @@ -0,0 +1,89 @@ +/** + * @file trajopt_osqp_solver_profile.h + * @brief + * + * @author Levi Armstrong + * @date December 13, 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 TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace OsqpEigen +{ +class Settings; +} + +namespace boost::serialization +{ +template +void serialize(Archive& ar, OsqpEigen::Settings& osqp_eigen_settings, const unsigned int version); // NOLINT + +template +void serialize(Archive& ar, trajopt_sqp::SQPParameters& params, const unsigned int version); // NOLINT + +} // namespace boost::serialization + +namespace tesseract_planning +{ +/** @brief The contains the default solver parameters available for setting up TrajOpt */ +class TrajOptIfoptOSQPSolverProfile : public TrajOptIfoptSolverProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + TrajOptIfoptOSQPSolverProfile(); + ~TrajOptIfoptOSQPSolverProfile() override; + TrajOptIfoptOSQPSolverProfile(const TrajOptIfoptOSQPSolverProfile&) = delete; + TrajOptIfoptOSQPSolverProfile& operator=(const TrajOptIfoptOSQPSolverProfile&) = delete; + TrajOptIfoptOSQPSolverProfile(TrajOptIfoptOSQPSolverProfile&&) = delete; + TrajOptIfoptOSQPSolverProfile&& operator=(TrajOptIfoptOSQPSolverProfile&&) = delete; + + /** @brief The OSQP convex solver settings to use */ + std::unique_ptr qp_settings{ nullptr }; + + /** @brief Optimization parameters */ + trajopt_sqp::SQPParameters opt_params{}; + + std::unique_ptr create(bool verbose = false) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT + + /** @brief Optimization callbacks */ + virtual std::vector> createOptimizationCallbacks() const; +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptOSQPSolverProfile) + +#endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_OSQP_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h index 2aadefeb10..0a71ce5553 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -75,8 +76,6 @@ class TrajOptIfoptPlanProfile : public Profile const std::vector& active_links, int index) const = 0; - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template @@ -104,8 +103,6 @@ class TrajOptIfoptCompositeProfile : public Profile const std::vector& active_links, const std::vector& fixed_indices) const = 0; - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template @@ -120,16 +117,14 @@ class TrajOptIfoptSolverProfile : public Profile TrajOptIfoptSolverProfile(); + virtual std::unique_ptr create(bool verbose = false) const = 0; + /** * @brief A utility function for getting profile ID * @return The profile ID used when storing in profile dictionary */ static std::size_t getStaticKey(); - virtual void apply(TrajOptIfoptProblem& problem) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; - protected: friend class boost::serialization::access; template diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h index d7d4a9664f..634c351af4 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_problem.h @@ -60,10 +60,6 @@ struct TrajOptIfoptProblem EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP - TrajOptIfoptProblem(); - - trajopt_sqp::SQPParameters opt_info; - // These are required for Tesseract to configure Descartes std::shared_ptr environment; tesseract_scene_graph::SceneState env_state; @@ -71,13 +67,6 @@ struct TrajOptIfoptProblem // Kinematic Objects std::shared_ptr manip; - std::vector> callbacks; - - /** @brief The OSQP convex solver settings to use */ - std::unique_ptr convex_solver_settings{ nullptr }; - - std::shared_ptr qp_solver; - std::shared_ptr nlp; std::vector> vars; }; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp index 0a9ae17a8f..386f8db151 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp @@ -26,7 +26,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include @@ -98,11 +97,6 @@ void TrajOptIfoptDefaultCompositeProfile::apply(TrajOptIfoptProblem& problem, addJointJerkSquaredCost(*problem.nlp, vars, jerk_coeff); } -tinyxml2::XMLElement* TrajOptIfoptDefaultCompositeProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const -{ - throw std::runtime_error("TrajOptIfoptDefaultCompositeProfile::toXML is not implemented!"); -} - template void TrajOptIfoptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index 4627f90d3c..7febedca55 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -26,7 +26,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include @@ -131,11 +130,6 @@ void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, } } -tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const -{ - throw std::runtime_error("TrajOptIfoptDefaultPlanProfile::toXML is not implemented!"); -} - template void TrajOptIfoptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) { diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp deleted file mode 100644 index e6471c6ddd..0000000000 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file trajopt_default_solver_profile.cpp - * @brief - * - * @author Levi Armstrong - * @date December 13, 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 -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -namespace tesseract_planning -{ -TrajOptIfoptDefaultSolverProfile::TrajOptIfoptDefaultSolverProfile() -{ - convex_solver_settings = std::make_unique(); - convex_solver_settings->setVerbosity(false); - convex_solver_settings->setWarmStart(true); - convex_solver_settings->setPolish(true); - convex_solver_settings->setAdaptiveRho(true); - convex_solver_settings->setMaxIteration(8192); - convex_solver_settings->setAbsoluteTolerance(1e-4); - convex_solver_settings->setRelativeTolerance(1e-6); -} - -TrajOptIfoptDefaultSolverProfile::~TrajOptIfoptDefaultSolverProfile() = default; - -void TrajOptIfoptDefaultSolverProfile::apply(TrajOptIfoptProblem& problem) const -{ - copyOSQPEigenSettings(*problem.convex_solver_settings, *convex_solver_settings); - problem.opt_info = opt_info; - problem.callbacks = callbacks; -} - -tinyxml2::XMLElement* TrajOptIfoptDefaultSolverProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const -{ - throw std::runtime_error("TrajOptIfoptDefaultSolverProfile::toXML is not implemented!"); -} - -template -void TrajOptIfoptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) -{ - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptSolverProfile); - /** @todo FIX */ - // ar& BOOST_SERIALIZATION_NVP(convex_solver_settings); - // ar& BOOST_SERIALIZATION_NVP(opt_info); - // ar& BOOST_SERIALIZATION_NVP(callbacks); -} - -} // namespace tesseract_planning - -#include -TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultSolverProfile) -BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultSolverProfile) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_osqp_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_osqp_solver_profile.cpp new file mode 100644 index 0000000000..8e3932047f --- /dev/null +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_osqp_solver_profile.cpp @@ -0,0 +1,155 @@ +/** + * @file trajopt_ifopt_osqp_solver_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date December 13, 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 +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace boost::serialization +{ +template +void serialize(Archive& ar, OsqpEigen::Settings& osqp_eigen_settings, const unsigned int /*version*/) +{ + OSQPSettings& settings = *osqp_eigen_settings.getSettings(); + ar& boost::serialization::make_nvp("rho", settings.rho); + ar& boost::serialization::make_nvp("sigma", settings.sigma); + ar& boost::serialization::make_nvp("scaling", settings.scaling); + ar& boost::serialization::make_nvp("adaptive_rho", settings.adaptive_rho); + ar& boost::serialization::make_nvp("adaptive_rho_interval", settings.adaptive_rho_interval); + ar& boost::serialization::make_nvp("adaptive_rho_tolerance", settings.adaptive_rho_tolerance); + ar& boost::serialization::make_nvp("adaptive_rho_fraction", settings.adaptive_rho_fraction); + ar& boost::serialization::make_nvp("max_iter", settings.max_iter); + ar& boost::serialization::make_nvp("eps_abs", settings.eps_abs); + ar& boost::serialization::make_nvp("eps_rel", settings.eps_rel); + ar& boost::serialization::make_nvp("eps_prim_inf", settings.eps_prim_inf); + ar& boost::serialization::make_nvp("eps_dual_inf", settings.eps_dual_inf); + ar& boost::serialization::make_nvp("alpha", settings.alpha); + ar& boost::serialization::make_nvp("linsys_solver", settings.linsys_solver); + ar& boost::serialization::make_nvp("delta", settings.delta); + ar& boost::serialization::make_nvp("polish", settings.polish); + ar& boost::serialization::make_nvp("polish_refine_iter", settings.polish_refine_iter); + ar& boost::serialization::make_nvp("verbose", settings.verbose); + ar& boost::serialization::make_nvp("scaled_termination", settings.scaled_termination); + ar& boost::serialization::make_nvp("check_termination", settings.check_termination); + ar& boost::serialization::make_nvp("warm_start", settings.warm_start); + ar& boost::serialization::make_nvp("time_limit", settings.time_limit); +} + +template +void serialize(Archive& ar, trajopt_sqp::SQPParameters& params, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("improve_ratio_threshold", params.improve_ratio_threshold); + ar& boost::serialization::make_nvp("min_trust_box_size", params.min_trust_box_size); + ar& boost::serialization::make_nvp("min_approx_improve", params.min_approx_improve); + ar& boost::serialization::make_nvp("min_approx_improve_frac", params.min_approx_improve_frac); + ar& boost::serialization::make_nvp("max_iter", params.max_iterations); + ar& boost::serialization::make_nvp("trust_shrink_ratio", params.trust_shrink_ratio); + ar& boost::serialization::make_nvp("trust_expand_ratio", params.trust_expand_ratio); + ar& boost::serialization::make_nvp("cnt_tolerance", params.cnt_tolerance); + ar& boost::serialization::make_nvp("max_merit_coeff_increases", params.max_merit_coeff_increases); + ar& boost::serialization::make_nvp("max_qp_solver_failures", params.max_qp_solver_failures); + ar& boost::serialization::make_nvp("merit_coeff_increase_ratio", params.merit_coeff_increase_ratio); + ar& boost::serialization::make_nvp("max_time", params.max_time); + ar& boost::serialization::make_nvp("initial_merit_error_coeff", params.initial_merit_error_coeff); + ar& boost::serialization::make_nvp("inflate_constraints_individually", params.inflate_constraints_individually); + ar& boost::serialization::make_nvp("trust_box_size", params.initial_trust_box_size); + ar& boost::serialization::make_nvp("log_results", params.log_results); + ar& boost::serialization::make_nvp("log_dir", params.log_dir); + // ar& boost::serialization::make_nvp("num_threads", params.num_threads); +} +} // namespace boost::serialization + +namespace tesseract_planning +{ +TrajOptIfoptOSQPSolverProfile::TrajOptIfoptOSQPSolverProfile() +{ + qp_settings = std::make_unique(); + qp_settings->setVerbosity(false); + qp_settings->setWarmStart(true); + qp_settings->setPolish(true); + qp_settings->setAdaptiveRho(true); + qp_settings->setMaxIteration(8192); + qp_settings->setAbsoluteTolerance(1e-4); + qp_settings->setRelativeTolerance(1e-6); +} + +TrajOptIfoptOSQPSolverProfile::~TrajOptIfoptOSQPSolverProfile() = default; + +std::unique_ptr TrajOptIfoptOSQPSolverProfile::create(bool verbose) const +{ + // Create QP Solver + auto qp_solver = std::make_shared(); + + // There seems to be no way to set objects solver_->settings() (OsqpEigen::Settings) + // or solver_->settings()->getSettings() (OSQPSettings) at once + copyOSQPEigenSettings(*qp_solver->solver_->settings(), *qp_settings); + qp_solver->solver_->settings()->setVerbosity((qp_settings->getSettings()->verbose != 0) || verbose); + + auto solver = std::make_unique(qp_solver); + solver->params = opt_params; + solver->verbose = verbose; + + // Add all callbacks + std::vector> callbacks = createOptimizationCallbacks(); + for (const trajopt_sqp::SQPCallback::Ptr& callback : callbacks) + solver->registerCallback(callback); + + return solver; +} + +std::vector> +TrajOptIfoptOSQPSolverProfile::createOptimizationCallbacks() const +{ + return {}; +} + +template +void TrajOptIfoptOSQPSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptSolverProfile); + ar& BOOST_SERIALIZATION_NVP(qp_settings); + ar& BOOST_SERIALIZATION_NVP(opt_params); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(OsqpEigen::Settings) +TESSERACT_SERIALIZE_FREE_ARCHIVES_INSTANTIATE(trajopt_sqp::SQPParameters) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptOSQPSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptOSQPSolverProfile) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 2d2c3d3b23..7efc9d7ae4 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -28,10 +28,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include -#include -#include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -39,7 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -105,33 +102,23 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) response.data = problem; } - // Create optimizer - /** @todo Enable solver selection (e.g. IPOPT) */ - auto qp_solver = std::make_shared(); - - // There seems to be no way to set objects solver_->settings() (OsqpEigen::Settings) - // or solver_->settings()->getSettings() (OSQPSettings) at once - copyOSQPEigenSettings(*qp_solver->solver_->settings(), *problem->convex_solver_settings); - qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings->getSettings()->verbose != 0) || - request.verbose); - - problem->qp_solver = qp_solver; + // Create Solver + TrajOptIfoptSolverProfile::ConstPtr solver_profile = + getProfile(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared()); - trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); - solver.params = problem->opt_info; + if (!solver_profile) + throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile"); - // Add all callbacks - for (const trajopt_sqp::SQPCallback::Ptr& callback : problem->callbacks) - { - solver.registerCallback(callback); - } + std::unique_ptr solver = solver_profile->create(request.verbose); - // solve - solver.verbose = request.verbose; - solver.solve(problem->nlp); + // Solver + solver->solve(problem->nlp); // Check success - if (solver.getStatus() != trajopt_sqp::SQPStatus::NLP_CONVERGED) + if (solver->getStatus() != trajopt_sqp::SQPStatus::NLP_CONVERGED) { response.successful = false; response.message = ERROR_FAILED_TO_FIND_VALID_SOLUTION; @@ -225,17 +212,6 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co throw std::runtime_error(error_msg); } - // Apply Solver parameters - TrajOptIfoptSolverProfile::ConstPtr solver_profile = - getProfile(name_, - request.instructions.getProfile(name_), - *request.profiles, - std::make_shared()); - if (!solver_profile) - throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile"); - - solver_profile->apply(*problem); - // Get kinematics information tesseract_environment::Environment::ConstPtr env = request.env; std::vector active_links = problem->manip->getActiveLinkNames(); diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp deleted file mode 100644 index 912af85a83..0000000000 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file trajopt_ifopt_problem.cpp - * @brief - * - * @author Levi Armstrong - * @date June 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 -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -TrajOptIfoptProblem::TrajOptIfoptProblem() -{ - convex_solver_settings = std::make_unique(); - convex_solver_settings->setVerbosity(false); - convex_solver_settings->setWarmStart(true); - convex_solver_settings->setPolish(true); - convex_solver_settings->setAdaptiveRho(true); - convex_solver_settings->setMaxIteration(8192); - convex_solver_settings->setAbsoluteTolerance(1e-4); - convex_solver_settings->setRelativeTolerance(1e-6); -} - -} // namespace tesseract_planning