From 445f67b96c9404dc2d03724e6ce48449245e97ed Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 15:48:54 +0100 Subject: [PATCH] - Added new TrajoptIfopt solver profiles - Fixed car_seat_example - Added debug option to most examples --- .../tesseract_examples/car_seat_example.h | 4 ++- .../pick_and_place_example.h | 2 ++ .../puzzle_piece_auxillary_axes_example.h | 4 ++- .../tesseract_examples/puzzle_piece_example.h | 4 ++- .../src/basic_cartesian_example.cpp | 2 ++ tesseract_examples/src/car_seat_example.cpp | 24 +++++++++---- .../src/freespace_hybrid_example.cpp | 2 ++ .../src/glass_upright_example.cpp | 2 ++ .../src/pick_and_place_example.cpp | 36 +++++++++++++------ .../puzzle_piece_auxillary_axes_example.cpp | 33 ++++++++++++----- .../src/puzzle_piece_example.cpp | 33 +++++++++++------ 11 files changed, 106 insertions(+), 40 deletions(-) diff --git a/tesseract_examples/include/tesseract_examples/car_seat_example.h b/tesseract_examples/include/tesseract_examples/car_seat_example.h index 61ab023a88..2ea0332de7 100644 --- a/tesseract_examples/include/tesseract_examples/car_seat_example.h +++ b/tesseract_examples/include/tesseract_examples/car_seat_example.h @@ -44,7 +44,8 @@ class CarSeatExample : public Example public: CarSeatExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, - bool ifopt = false); + bool ifopt = false, + bool debug = false); ~CarSeatExample() override = default; CarSeatExample(const CarSeatExample&) = default; CarSeatExample& operator=(const CarSeatExample&) = default; @@ -55,6 +56,7 @@ class CarSeatExample : public Example private: bool ifopt_; + bool debug_; std::unordered_map> saved_positions_; static std::unordered_map> getPredefinedPosition(); diff --git a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h index 7dafe34607..0270c42924 100644 --- a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h +++ b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h @@ -46,6 +46,7 @@ class PickAndPlaceExample : public Example PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, bool ifopt = false, + bool debug = false, double box_size = 0.2, std::array box_position = { 0.15, 0.4 }); ~PickAndPlaceExample() override = default; @@ -58,6 +59,7 @@ class PickAndPlaceExample : public Example private: bool ifopt_; + bool debug_; double box_size_; std::array box_position_; static tesseract_environment::Command::Ptr addBox(double box_x, double box_y, double box_side); diff --git a/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h b/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h index b7866971e8..ad869bd0b8 100644 --- a/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h +++ b/tesseract_examples/include/tesseract_examples/puzzle_piece_auxillary_axes_example.h @@ -48,7 +48,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example public: PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, - bool ifopt = false); + bool ifopt = false, + bool debug = false); ~PuzzlePieceAuxillaryAxesExample() override = default; PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default; PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default; @@ -59,6 +60,7 @@ class PuzzlePieceAuxillaryAxesExample : public Example private: bool ifopt_; + bool debug_; static tesseract_common::VectorIsometry3d makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator); }; diff --git a/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h b/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h index 62ac0eacc7..8a327fb6b9 100644 --- a/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h +++ b/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h @@ -45,7 +45,8 @@ class PuzzlePieceExample : public Example public: PuzzlePieceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, - bool ifopt = false); + bool ifopt = false, + bool debug = false); ~PuzzlePieceExample() override = default; PuzzlePieceExample(const PuzzlePieceExample&) = default; PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default; @@ -56,6 +57,7 @@ class PuzzlePieceExample : public Example private: bool ifopt_; + bool debug_; static tesseract_common::VectorIsometry3d makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator); }; diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 5fe07d2b45..713ce6fd9a 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -152,6 +152,8 @@ bool BasicCartesianExample::run() if (debug_) console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); // Create Task Composer Plugin Factory const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 16a335ae85..611f151433 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -49,6 +49,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -116,8 +117,9 @@ Commands addSeats(const tesseract_common::ResourceLocator::ConstPtr& locator) CarSeatExample::CarSeatExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, - bool ifopt) - : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) + bool ifopt, + bool debug) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug) { } @@ -219,7 +221,10 @@ Eigen::VectorXd CarSeatExample::getPositionVectorXd(const JointGroup& joint_grou bool CarSeatExample::run() { - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + if (debug_) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); // Create Task Composer Plugin Factory const std::string share_dir(TESSERACT_TASK_COMPOSER_DIR); @@ -256,19 +261,19 @@ bool CarSeatExample::run() // Create TrajOpt_Ifopt Profile auto trajopt_ifopt_composite_profile = std::make_shared(); trajopt_ifopt_composite_profile->collision_constraint_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.00); trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005; trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data = trajopt_common::CollisionCoeffData(1); trajopt_ifopt_composite_profile->collision_cost_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.005); trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01; trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = - trajopt_common::CollisionCoeffData(5); + trajopt_common::CollisionCoeffData(50); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; @@ -281,10 +286,17 @@ 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; + profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile); } else { diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 9ddcf3cb97..9a15d112b9 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -103,6 +103,8 @@ bool FreespaceHybridExample::run() { if (debug_) console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); // Add sphere to environment Command::Ptr cmd = addSphere(); diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 83be6b007f..213e80cfdd 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -140,6 +140,8 @@ bool GlassUprightExample::run() if (debug_) console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); // Solve Trajectory CONSOLE_BRIDGE_logInform("glass upright plan example"); diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 25ec9836fe..a29bd82747 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -24,8 +24,6 @@ * limitations under the License. */ #include -#include -#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include @@ -36,6 +34,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include #include #include #include @@ -72,9 +73,14 @@ namespace tesseract_examples PickAndPlaceExample::PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, bool ifopt, + bool debug, double box_size, std::array box_position) - : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), box_size_(box_size), box_position_(box_position) + : Example(std::move(env), std::move(plotter)) + , ifopt_(ifopt) + , debug_(debug) + , box_size_(box_size) + , box_position_(box_position) { } @@ -108,7 +114,10 @@ bool PickAndPlaceExample::run() /// SETUP /// ///////////// - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + if (debug_) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); // Set default contact distance Command::Ptr cmd_default_dist = std::make_shared(0.005); @@ -200,16 +209,20 @@ bool PickAndPlaceExample::run() if (ifopt_) { // Create TrajOpt_Ifopt Profile + auto trajopt_ifopt_plan_profile = std::make_shared(); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); + trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); + auto trajopt_ifopt_composite_profile = std::make_shared(); trajopt_ifopt_composite_profile->collision_constraint_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.00); trajopt_ifopt_composite_profile->collision_constraint_config->collision_margin_buffer = 0.005; trajopt_ifopt_composite_profile->collision_constraint_config->collision_coeff_data = trajopt_common::CollisionCoeffData(1); trajopt_ifopt_composite_profile->collision_cost_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.005); trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.01; @@ -223,14 +236,15 @@ 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_plan_profile = std::make_shared(); - trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); - trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); + auto trajopt_ifopt_solver_profile = std::make_shared(); + trajopt_ifopt_solver_profile->opt_info.max_iterations = 100; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index e9f540a501..bdee176237 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -126,14 +127,18 @@ PuzzlePieceAuxillaryAxesExample::makePuzzleToolPoses(const tesseract_common::Res PuzzlePieceAuxillaryAxesExample::PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, - bool ifopt) - : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) + bool ifopt, + bool debug) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug) { } bool PuzzlePieceAuxillaryAxesExample::run() { - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + if (debug_) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); if (plotter_ != nullptr) plotter_->waitForConnection(); @@ -203,6 +208,13 @@ bool PuzzlePieceAuxillaryAxesExample::run() if (ifopt_) { // Create TrajOpt_Ifopt Profile + auto trajopt_ifopt_plan_profile = std::make_shared(); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); + trajopt_ifopt_plan_profile->cartesian_coeff(3) = 2; + trajopt_ifopt_plan_profile->cartesian_coeff(4) = 2; + trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0; + trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); + auto trajopt_ifopt_composite_profile = std::make_shared(); trajopt_ifopt_composite_profile->collision_constraint_config = nullptr; trajopt_ifopt_composite_profile->collision_cost_config->type = @@ -219,15 +231,18 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_ifopt_composite_profile->smooth_jerks = true; trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - auto trajopt_ifopt_plan_profile = std::make_shared(); - trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0; - trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); + 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; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 9c8f6c120c..b1020c0ef8 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -126,14 +127,18 @@ PuzzlePieceExample::makePuzzleToolPoses(const tesseract_common::ResourceLocator: PuzzlePieceExample::PuzzlePieceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, - bool ifopt) - : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) + bool ifopt, + bool debug) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug) { } bool PuzzlePieceExample::run() { - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); + if (debug_) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + else + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); if (plotter_ != nullptr) plotter_->waitForConnection(); @@ -199,15 +204,19 @@ bool PuzzlePieceExample::run() if (ifopt_) { // Create TrajOpt_Ifopt Profile + auto trajopt_ifopt_plan_profile = std::make_shared(); + trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); + trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0; + trajopt_ifopt_plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); + auto trajopt_ifopt_composite_profile = std::make_shared(); trajopt_ifopt_composite_profile->collision_constraint_config = nullptr; trajopt_ifopt_composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::DISCRETE; trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = tesseract_collision::ContactManagerConfig(0.025); - trajopt_ifopt_composite_profile->collision_cost_config->collision_margin_buffer = 0.05; trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = - trajopt_common::CollisionCoeffData(2); + trajopt_common::CollisionCoeffData(20); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; @@ -215,15 +224,17 @@ bool PuzzlePieceExample::run() trajopt_ifopt_composite_profile->smooth_jerks = true; trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - auto trajopt_ifopt_plan_profile = std::make_shared(); - trajopt_ifopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - trajopt_ifopt_plan_profile->cartesian_coeff(5) = 0; - 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; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else {