From 41dee76e5dd99de89d9f04e7e4ff60636250aa93 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 3 Jun 2024 19:00:00 +0200 Subject: [PATCH] Add convex_solver_settings to TrajOptIfoptDefaultSolverProfile (#425) Co-authored-by: Levi Armstrong --- .../trajopt_ifopt/CMakeLists.txt | 1 + .../trajopt_ifopt_default_solver_profile.h | 21 +++++--- .../trajopt_ifopt/trajopt_ifopt_problem.h | 13 ++++- .../trajopt_ifopt/trajopt_ifopt_utils.h | 13 +++-- .../trajopt_ifopt_default_solver_profile.cpp | 18 ++++++- .../src/trajopt_ifopt_motion_planner.cpp | 19 ++++---- .../src/trajopt_ifopt_problem.cpp | 48 +++++++++++++++++++ .../trajopt_ifopt/src/trajopt_ifopt_utils.cpp | 34 +++++++++++-- 8 files changed, 144 insertions(+), 23 deletions(-) create mode 100644 tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp diff --git a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt index 9d14cb8d14c..711c515ac29 100644 --- a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt @@ -5,6 +5,7 @@ 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_default_plan_profile.cpp src/profile/trajopt_ifopt_default_composite_profile.cpp 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 index feb882dc353..80be4dbb703 100644 --- 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 @@ -35,6 +35,11 @@ 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 */ @@ -44,15 +49,19 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptDefaultSolverProfile() = default; - ~TrajOptIfoptDefaultSolverProfile() override = default; - TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; - TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; + TrajOptIfoptDefaultSolverProfile(); + ~TrajOptIfoptDefaultSolverProfile() override; + TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete; + TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete; TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; - /** @brief Optimization paramters */ - trajopt_sqp::SQPParameters opt_info; + /** @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; 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 005e1e04cde..d7d4a9664fe 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 @@ -30,7 +30,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include #include #include #include @@ -41,6 +40,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +namespace OsqpEigen +{ +class Settings; +} + namespace tesseract_planning { enum class TrajOptIfoptTermType @@ -56,6 +60,8 @@ struct TrajOptIfoptProblem EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP + TrajOptIfoptProblem(); + trajopt_sqp::SQPParameters opt_info; // These are required for Tesseract to configure Descartes @@ -67,6 +73,11 @@ struct TrajOptIfoptProblem 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/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h index c84216027c4..98cfac23198 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/trajopt_ifopt_utils.h @@ -42,8 +42,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +namespace OsqpEigen +{ +class Settings; +} + namespace tesseract_planning { +void copyOSQPEigenSettings(OsqpEigen::Settings& lhs, const OsqpEigen::Settings& rhs); + bool addCartesianPositionConstraint(trajopt_sqp::QPProblem& nlp, const std::shared_ptr& var, const std::shared_ptr& manip, @@ -73,17 +80,17 @@ bool addCartesianPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp, bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs); bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs); bool addJointPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs); bool addCollisionConstraint(trajopt_sqp::QPProblem& nlp, 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 index 1071a687d99..c7e149fa93c 100644 --- 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 @@ -27,16 +27,32 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #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; } 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 8fa4e3dfa8a..52dc9122803 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 @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -107,16 +108,16 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) // Create optimizer /** @todo Enable solver selection (e.g. IPOPT) */ auto qp_solver = std::make_shared(); - trajopt_sqp::TrustRegionSQPSolver solver(qp_solver); - /** @todo Set these as the defaults in trajopt and allow setting */ - qp_solver->solver_->settings()->setVerbosity(request.verbose); - 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); + // 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; + + trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; // Add all callbacks diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp new file mode 100644 index 00000000000..912af85a831 --- /dev/null +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_problem.cpp @@ -0,0 +1,48 @@ +/** + * @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 diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp index ea7d5bd3258..6d2921962fd 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_utils.cpp @@ -42,6 +42,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -58,6 +59,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +void copyOSQPEigenSettings(OsqpEigen::Settings& lhs, const OsqpEigen::Settings& rhs) +{ + const auto& settings = *rhs.getSettings(); + lhs.setRho(settings.rho); + lhs.setSigma(settings.sigma); + lhs.setScaling(static_cast(settings.scaling)); + lhs.setAdaptiveRho(static_cast(settings.adaptive_rho)); + lhs.setAdaptiveRhoInterval(static_cast(settings.adaptive_rho_interval)); + lhs.setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance); + lhs.setAdaptiveRhoFraction(settings.adaptive_rho_fraction); + lhs.setMaxIteration(static_cast(settings.max_iter)); + lhs.setAbsoluteTolerance(settings.eps_abs); + lhs.setRelativeTolerance(settings.eps_rel); + lhs.setPrimalInfeasibilityTollerance(settings.eps_prim_inf); + lhs.setDualInfeasibilityTollerance(settings.eps_dual_inf); + lhs.setAlpha(settings.alpha); + lhs.setLinearSystemSolver(settings.linsys_solver); + lhs.setDelta(settings.delta); + lhs.setPolish(static_cast(settings.polish)); + lhs.setPolishRefineIter(static_cast(settings.polish_refine_iter)); + lhs.setVerbosity(static_cast(settings.verbose)); + lhs.setScaledTerimination(static_cast(settings.scaled_termination)); + lhs.setCheckTermination(static_cast(settings.check_termination)); + lhs.setWarmStart(static_cast(settings.warm_start)); + lhs.setTimeLimit(settings.time_limit); +} + ifopt::ConstraintSet::Ptr createCartesianPositionConstraint(const std::shared_ptr& var, const std::shared_ptr& manip, @@ -380,7 +408,7 @@ bool addCartesianPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp, bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs) { auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs); @@ -390,7 +418,7 @@ bool addJointPositionConstraint(trajopt_sqp::QPProblem& nlp, bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs) { auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs); @@ -399,7 +427,7 @@ bool addJointPositionSquaredCost(trajopt_sqp::QPProblem& nlp, } bool addJointPositionAbsoluteCost(trajopt_sqp::QPProblem& nlp, const JointWaypointPoly& joint_waypoint, - const trajopt_ifopt::JointPosition::ConstPtr& var, + const std::shared_ptr& var, const Eigen::VectorXd& coeffs) { auto constraint = createJointPositionConstraint(joint_waypoint, var, coeffs);