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..f1b5b523281 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,7 +50,7 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using ConstPtr = std::shared_ptr; TrajOptIfoptDefaultSolverProfile(); - ~TrajOptIfoptDefaultSolverProfile() override = default; + ~TrajOptIfoptDefaultSolverProfile() override; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; @@ -54,7 +58,7 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile /** @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..a1d88792374 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(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(settings.polish); + lhs.setPolishRefineIter(static_cast(settings.polish_refine_iter)); + lhs.setVerbosity(settings.verbose); + lhs.setScaledTerimination(settings.scaled_termination); + lhs.setCheckTermination(static_cast(settings.check_termination)); + lhs.setWarmStart(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);