From 356f130ac4c513a0d3bd880a05042fa03ff8a80d Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 16:35:29 +0100 Subject: [PATCH 1/2] - Added convex_solver_settings with proper defaults to TrajOptIfoptDefaultSolverProfile - Added handling of convex_solver_settings to trajopt_ifopt_motion_planner --- .../trajopt_ifopt_default_solver_profile.h | 11 +++-- .../trajopt_ifopt/trajopt_ifopt_problem.h | 10 ++++- .../trajopt_ifopt_default_solver_profile.cpp | 15 ++++++- .../src/trajopt_ifopt_motion_planner.cpp | 41 +++++++++++++++---- 4 files changed, 63 insertions(+), 14 deletions(-) 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..323472fe5dc 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 @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -44,15 +45,19 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptDefaultSolverProfile() = default; + TrajOptIfoptDefaultSolverProfile(); ~TrajOptIfoptDefaultSolverProfile() override = default; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; 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 */ + OSQPSettings convex_solver_settings{}; + + /** @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..15e61bd5d71 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,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include +#include #include #include #include @@ -56,6 +56,8 @@ struct TrajOptIfoptProblem EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP + TrajOptIfoptProblem() { osqp_set_default_settings(&convex_solver_settings); }; + trajopt_sqp::SQPParameters opt_info; // These are required for Tesseract to configure Descartes @@ -67,6 +69,12 @@ struct TrajOptIfoptProblem std::vector> callbacks; + /** @brief The OSQP convex solver settings to use + * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ + OSQPSettings convex_solver_settings{}; + + 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_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp index 1071a687d99..79fbd615acc 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,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -35,8 +34,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { + +TrajOptIfoptDefaultSolverProfile::TrajOptIfoptDefaultSolverProfile() +{ + osqp_set_default_settings(&convex_solver_settings); + convex_solver_settings.verbose = 0; + convex_solver_settings.warm_start = 1; + convex_solver_settings.polish = 1; + convex_solver_settings.adaptive_rho = 1; + convex_solver_settings.max_iter = 8192; + convex_solver_settings.eps_abs = 1e-4; + convex_solver_settings.eps_rel = 1e-6; +} + void TrajOptIfoptDefaultSolverProfile::apply(TrajOptIfoptProblem& problem) const { + 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..e64759a05f4 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 @@ -71,6 +71,34 @@ std::unique_ptr TrajOptIfoptMotionPlanner::clone() const return std::make_unique(name_); } +void OSQPEigenSolver_setSettings(const std::unique_ptr& solver_, OSQPSettings settings) +{ + // There seems to be no way to set objects solver_->settings() (OsqpEigen::Settings) + // or solver_->settings()->getSettings() (OSQPSettings) at once + solver_->settings()->setRho(settings.rho); + solver_->settings()->setSigma(settings.sigma); + solver_->settings()->setScaling((int)settings.scaling); + solver_->settings()->setAdaptiveRho(settings.adaptive_rho); + solver_->settings()->setAdaptiveRhoInterval((int)settings.adaptive_rho_interval); + solver_->settings()->setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance); + solver_->settings()->setAdaptiveRhoFraction(settings.adaptive_rho_fraction); + solver_->settings()->setMaxIteration((int)settings.max_iter); + solver_->settings()->setAbsoluteTolerance(settings.eps_abs); + solver_->settings()->setRelativeTolerance(settings.eps_rel); + solver_->settings()->setPrimalInfeasibilityTollerance(settings.eps_prim_inf); + solver_->settings()->setDualInfeasibilityTollerance(settings.eps_dual_inf); + solver_->settings()->setAlpha(settings.alpha); + solver_->settings()->setLinearSystemSolver(settings.linsys_solver); + solver_->settings()->setDelta(settings.delta); + solver_->settings()->setPolish(settings.polish); + solver_->settings()->setPolishRefineIter((int)settings.polish_refine_iter); + solver_->settings()->setVerbosity(settings.verbose); + solver_->settings()->setScaledTerimination(settings.scaled_termination); + solver_->settings()->setCheckTermination((int)settings.check_termination); + solver_->settings()->setWarmStart(settings.warm_start); + solver_->settings()->setTimeLimit(settings.time_limit); +} + PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; @@ -107,16 +135,11 @@ 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); + OSQPEigenSolver_setSettings(qp_solver->solver_, problem->convex_solver_settings); + qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); + problem->qp_solver = qp_solver; + trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; // Add all callbacks From b30da3033c77597b5c215318467e62eee77723fe Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 2 Jun 2024 21:11:28 -0500 Subject: [PATCH 2/2] Update to leverage osqp eigen settings object --- .../trajopt_ifopt/CMakeLists.txt | 1 + .../trajopt_ifopt_default_solver_profile.h | 14 ++++-- .../trajopt_ifopt/trajopt_ifopt_problem.h | 13 +++-- .../trajopt_ifopt/trajopt_ifopt_utils.h | 13 +++-- .../trajopt_ifopt_default_solver_profile.cpp | 23 +++++---- .../src/trajopt_ifopt_motion_planner.cpp | 38 ++++----------- .../src/trajopt_ifopt_problem.cpp | 48 +++++++++++++++++++ .../trajopt_ifopt/src/trajopt_ifopt_utils.cpp | 34 +++++++++++-- 8 files changed, 128 insertions(+), 56 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 323472fe5dc..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 @@ -29,13 +29,17 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include -#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 */ @@ -46,15 +50,15 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using ConstPtr = std::shared_ptr; TrajOptIfoptDefaultSolverProfile(); - ~TrajOptIfoptDefaultSolverProfile() override = default; - TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; - TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; + ~TrajOptIfoptDefaultSolverProfile() override; + TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete; + TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete; TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; /** @brief The OSQP convex solver settings to use * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ - OSQPSettings convex_solver_settings{}; + std::unique_ptr convex_solver_settings{ nullptr }; /** @brief Optimization parameters */ trajopt_sqp::SQPParameters opt_info{}; 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 15e61bd5d71..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,7 +60,7 @@ struct TrajOptIfoptProblem EIGEN_MAKE_ALIGNED_OPERATOR_NEW // LCOV_EXCL_STOP - TrajOptIfoptProblem() { osqp_set_default_settings(&convex_solver_settings); }; + TrajOptIfoptProblem(); trajopt_sqp::SQPParameters opt_info; @@ -69,9 +73,8 @@ struct TrajOptIfoptProblem std::vector> callbacks; - /** @brief The OSQP convex solver settings to use - * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ - OSQPSettings convex_solver_settings{}; + /** @brief The OSQP convex solver settings to use */ + std::unique_ptr convex_solver_settings{ nullptr }; std::shared_ptr qp_solver; 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 79fbd615acc..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,29 +27,32 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_planning { - TrajOptIfoptDefaultSolverProfile::TrajOptIfoptDefaultSolverProfile() { - osqp_set_default_settings(&convex_solver_settings); - convex_solver_settings.verbose = 0; - convex_solver_settings.warm_start = 1; - convex_solver_settings.polish = 1; - convex_solver_settings.adaptive_rho = 1; - convex_solver_settings.max_iter = 8192; - convex_solver_settings.eps_abs = 1e-4; - convex_solver_settings.eps_rel = 1e-6; + 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 { - problem.convex_solver_settings = convex_solver_settings; + 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 e64759a05f4..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 @@ -71,34 +72,6 @@ std::unique_ptr TrajOptIfoptMotionPlanner::clone() const return std::make_unique(name_); } -void OSQPEigenSolver_setSettings(const std::unique_ptr& solver_, OSQPSettings settings) -{ - // There seems to be no way to set objects solver_->settings() (OsqpEigen::Settings) - // or solver_->settings()->getSettings() (OSQPSettings) at once - solver_->settings()->setRho(settings.rho); - solver_->settings()->setSigma(settings.sigma); - solver_->settings()->setScaling((int)settings.scaling); - solver_->settings()->setAdaptiveRho(settings.adaptive_rho); - solver_->settings()->setAdaptiveRhoInterval((int)settings.adaptive_rho_interval); - solver_->settings()->setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance); - solver_->settings()->setAdaptiveRhoFraction(settings.adaptive_rho_fraction); - solver_->settings()->setMaxIteration((int)settings.max_iter); - solver_->settings()->setAbsoluteTolerance(settings.eps_abs); - solver_->settings()->setRelativeTolerance(settings.eps_rel); - solver_->settings()->setPrimalInfeasibilityTollerance(settings.eps_prim_inf); - solver_->settings()->setDualInfeasibilityTollerance(settings.eps_dual_inf); - solver_->settings()->setAlpha(settings.alpha); - solver_->settings()->setLinearSystemSolver(settings.linsys_solver); - solver_->settings()->setDelta(settings.delta); - solver_->settings()->setPolish(settings.polish); - solver_->settings()->setPolishRefineIter((int)settings.polish_refine_iter); - solver_->settings()->setVerbosity(settings.verbose); - solver_->settings()->setScaledTerimination(settings.scaled_termination); - solver_->settings()->setCheckTermination((int)settings.check_termination); - solver_->settings()->setWarmStart(settings.warm_start); - solver_->settings()->setTimeLimit(settings.time_limit); -} - PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; @@ -135,10 +108,15 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) // Create optimizer /** @todo Enable solver selection (e.g. IPOPT) */ auto qp_solver = std::make_shared(); - OSQPEigenSolver_setSettings(qp_solver->solver_, problem->convex_solver_settings); - qp_solver->solver_->settings()->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); + + // 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; 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);