From 6c52ae16f2cb2ec5abc034e66809b4321fdbffe3 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 14 Dec 2023 14:41:30 +0100 Subject: [PATCH 01/25] Add cartesian waypoints to fixed indices --- .../trajopt/src/trajopt_motion_planner.cpp | 4 +++- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 13905c9d6d..ec15dc41bc 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -273,7 +273,9 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const seed_states.push_back(request.env_state.getJointValues(joint_names)); } - /** @todo If fixed cartesian and not term_type cost add as fixed */ + // Add to fixed indices + if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + fixed_steps.push_back(i); } else if (move_instruction.getWaypoint().isJointWaypoint()) { 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 302c0daa76..a28cf4768c 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 @@ -289,7 +289,9 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply profile cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i); - /** @todo If fixed cartesian and not term_type cost add as fixed */ + // Add to fixed indices + if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ + fixed_steps.push_back(i); } else if (move_instruction.getWaypoint().isJointWaypoint()) { From 6b22fcb7ccc02d34a2688b63e9548938f84ceb49 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 16:35:29 +0100 Subject: [PATCH 02/25] - 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 | 15 +++++-- .../trajopt_ifopt/trajopt_ifopt_problem.h | 10 +++++ .../trajopt_ifopt_default_solver_profile.cpp | 14 +++++++ .../src/trajopt_ifopt_motion_planner.cpp | 42 ++++++++++++++----- 4 files changed, 68 insertions(+), 13 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 4675998603..24db86a6cd 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 @@ -26,6 +26,11 @@ #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +TRAJOPT_IGNORE_WARNINGS_POP + #include #include @@ -38,15 +43,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 84113c103c..e584d3be15 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,10 +30,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include namespace tesseract_planning @@ -51,6 +53,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 @@ -62,6 +66,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; + trajopt_sqp::QPProblem::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 3aa9ef80b8..de6646961b 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 @@ -28,8 +28,22 @@ 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 a28cf4768c..8e94fda09e 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 @@ -63,6 +63,34 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const return std::make_shared(name_); } +void OSQPEigenSolver_setSettings(const OsqpEigen::Solver& 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; @@ -97,17 +125,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); + problem->qp_solver = std::make_shared(); + OSQPEigenSolver_setSettings(dynamic_cast(*problem->qp_solver).solver_, + problem->convex_solver_settings); + trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; // Add all callbacks From 699c89f3b44dac08f77e081e9ae47e5dd1f14c20 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 18:08:42 +0100 Subject: [PATCH 03/25] fixup --- .../profile/trajopt_ifopt_default_solver_profile.h | 2 +- .../trajopt_ifopt/trajopt_ifopt_problem.h | 2 +- 2 files changed, 2 insertions(+), 2 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 24db86a6cd..bd81351e0e 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 @@ -28,7 +28,7 @@ #include TRAJOPT_IGNORE_WARNINGS_PUSH -#include +#include TRAJOPT_IGNORE_WARNINGS_POP #include 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 e584d3be15..da32e20b27 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 TESSERACT_COMMON_IGNORE_WARNINGS_POP #include From 2d21596ede60d6d36ccff403943ad470a2b188c2 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 21:59:16 +0100 Subject: [PATCH 04/25] Restore previous verbosity behaviour --- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 8e94fda09e..d4b1dd578e 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 @@ -128,6 +128,9 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) problem->qp_solver = std::make_shared(); OSQPEigenSolver_setSettings(dynamic_cast(*problem->qp_solver).solver_, problem->convex_solver_settings); + dynamic_cast(*problem->qp_solver) + .solver_.settings() + ->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); solver.params = problem->opt_info; From 2e9f2aeb1fbcfafa140b0a59943e80e35fd1adc2 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 2 Oct 2023 07:39:31 +0200 Subject: [PATCH 05/25] Added ifopt option to car_seat_example --- .clang-format | 1 - .../tesseract_examples/car_seat_example.h | 5 +- tesseract_examples/src/car_seat_example.cpp | 65 +++++++++++++------ .../src/car_seat_example_node.cpp | 2 +- 4 files changed, 49 insertions(+), 24 deletions(-) diff --git a/.clang-format b/.clang-format index 8b3d0c00da..792fd96a05 100644 --- a/.clang-format +++ b/.clang-format @@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false AllowShortFunctionsOnASingleLine: true AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false -AllowShortLoopsOnASingleLine: false AlwaysBreakBeforeMultilineStrings: false AlwaysBreakTemplateDeclarations: true BinPackArguments: false diff --git a/tesseract_examples/include/tesseract_examples/car_seat_example.h b/tesseract_examples/include/tesseract_examples/car_seat_example.h index 5a9fb8e5bd..61ab023a88 100644 --- a/tesseract_examples/include/tesseract_examples/car_seat_example.h +++ b/tesseract_examples/include/tesseract_examples/car_seat_example.h @@ -30,7 +30,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -44,7 +43,8 @@ class CarSeatExample : public Example { public: CarSeatExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter = nullptr); + tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false); ~CarSeatExample() override = default; CarSeatExample(const CarSeatExample&) = default; CarSeatExample& operator=(const CarSeatExample&) = default; @@ -54,6 +54,7 @@ class CarSeatExample : public Example bool run() override final; private: + bool ifopt_; std::unordered_map> saved_positions_; static std::unordered_map> getPredefinedPosition(); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 825b728748..ed74503240 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -46,6 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -58,6 +59,7 @@ using namespace tesseract_visualization; using namespace tesseract_planning; using tesseract_common::ManipulatorInfo; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; namespace tesseract_examples { @@ -112,8 +114,9 @@ Commands addSeats(const tesseract_common::ResourceLocator::ConstPtr& locator) } CarSeatExample::CarSeatExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter) - : Example(std::move(env), std::move(plotter)) + tesseract_visualization::Visualization::Ptr plotter, + bool ifopt) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) { } @@ -245,25 +248,46 @@ bool CarSeatExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - // Create TrajOpt Profile - auto trajopt_composite_profile = std::make_shared(); - trajopt_composite_profile->collision_constraint_config.enabled = true; - trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00; - trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; - trajopt_composite_profile->collision_constraint_config.coeff = 10; - trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; - trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; - trajopt_composite_profile->collision_cost_config.coeff = 50; - - auto trajopt_solver_profile = std::make_shared(); - trajopt_solver_profile->opt_info.max_iter = 200; - trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + if (ifopt_) + { + // Create TrajOpt Profile + auto trajopt_ifopt_composite_profile = std::make_shared(); + 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_ifopt::CollisionCoeffData(10); + 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->contact_manager_config = + tesseract_collision::ContactManagerConfig(50); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + } + else + { + // Create TrajOpt Profile + auto trajopt_composite_profile = std::make_shared(); + trajopt_composite_profile->collision_constraint_config.enabled = true; + trajopt_composite_profile->collision_constraint_config.safety_margin = 0.00; + trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; + trajopt_composite_profile->collision_constraint_config.coeff = 10; + trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; + trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; + trajopt_composite_profile->collision_cost_config.coeff = 50; + + auto trajopt_solver_profile = std::make_shared(); + trajopt_solver_profile->opt_info.max_iter = 200; + trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; + trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + } // Solve Trajectory CONSOLE_BRIDGE_logInform("Car Seat Demo Started"); @@ -297,7 +321,8 @@ bool CarSeatExample::run() CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example"); // Create task - TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().front(); // Create Task Composer Problem diff --git a/tesseract_examples/src/car_seat_example_node.cpp b/tesseract_examples/src/car_seat_example_node.cpp index 59405b4db2..9280711ec0 100644 --- a/tesseract_examples/src/car_seat_example_node.cpp +++ b/tesseract_examples/src/car_seat_example_node.cpp @@ -42,7 +42,7 @@ int main(int /*argc*/, char** /*argv*/) if (!env->init(urdf_path, srdf_path, locator)) exit(1); - CarSeatExample example(env, nullptr); + CarSeatExample example(env, nullptr, false); if (!example.run()) exit(1); } From 1eba9afa799c51be86dfc56ab9f25b6e6c490d2d Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Mon, 2 Oct 2023 09:13:26 +0200 Subject: [PATCH 06/25] Added missing parameters --- tesseract_examples/src/car_seat_example.cpp | 28 +++++++++++++++++---- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index ed74503240..cc9dfe86b7 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -46,8 +46,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include +#include +#include #include #include @@ -252,21 +253,37 @@ bool CarSeatExample::run() auto profiles = std::make_shared(); if (ifopt_) { - // Create TrajOpt Profile + // 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; 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_ifopt::CollisionCoeffData(10); + trajopt_ifopt::CollisionCoeffData(1); + trajopt_ifopt_composite_profile->collision_cost_config->type = + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; 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->contact_manager_config = - tesseract_collision::ContactManagerConfig(50); + tesseract_collision::ContactManagerConfig(5); + trajopt_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_accelerations = true; + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_jerks = true; + trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1; + + auto plan_profile = std::make_shared(); + plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); + plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", plan_profile); } else { @@ -406,7 +423,8 @@ bool CarSeatExample::run() CONSOLE_BRIDGE_logInform("Freespace plan to pick seat 1 example"); // Create task - TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().front(); // Create Task Composer Problem From 2af56e367cf94965f450ad49f320ac58272db928 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 6 Oct 2023 13:08:37 +0200 Subject: [PATCH 07/25] Fix collision_cost_config->collision_coeff_data --- tesseract_examples/src/car_seat_example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index cc9dfe86b7..27aa402657 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -267,8 +267,8 @@ bool CarSeatExample::run() 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->contact_manager_config = - tesseract_collision::ContactManagerConfig(5); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = + trajopt_ifopt::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; From 7928fcdb7af0d1207071daa5c8bf5daea1625164 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 6 Oct 2023 15:08:29 +0200 Subject: [PATCH 08/25] Added ifopt option to pick_and_place_example --- .../pick_and_place_example.h | 2 + tesseract_examples/src/car_seat_example.cpp | 12 +-- .../src/pick_and_place_example.cpp | 90 ++++++++++++++----- 3 files changed, 74 insertions(+), 30 deletions(-) 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 1b7e84453e..7dafe34607 100644 --- a/tesseract_examples/include/tesseract_examples/pick_and_place_example.h +++ b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h @@ -45,6 +45,7 @@ class PickAndPlaceExample : public Example public: PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false, double box_size = 0.2, std::array box_position = { 0.15, 0.4 }); ~PickAndPlaceExample() override = default; @@ -56,6 +57,7 @@ class PickAndPlaceExample : public Example bool run() override final; private: + bool ifopt_; 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/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 27aa402657..31dac95bc5 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -267,8 +267,7 @@ bool CarSeatExample::run() 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_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_ifopt::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; @@ -277,13 +276,14 @@ bool CarSeatExample::run() trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->longest_valid_segment_length = 0.1; - auto plan_profile = std::make_shared(); - plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); - plan_profile->joint_coeff = Eigen::VectorXd::Ones(8); + 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(8); profiles->addProfile( TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); } else { diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index c37d868359..36fe3854ec 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -24,6 +24,8 @@ * limitations under the License. */ #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include @@ -63,14 +65,16 @@ const std::string LINK_BOX_NAME = "box"; const std::string LINK_BASE_NAME = "world"; const std::string LINK_END_EFFECTOR_NAME = "iiwa_tool0"; const std::string DISCRETE_CONTACT_CHECK_TASK_NAME = "DiscreteContactCheckTask"; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; namespace tesseract_examples { PickAndPlaceExample::PickAndPlaceExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, + bool ifopt, double box_size, std::array box_position) - : Example(std::move(env), std::move(plotter)), box_size_(box_size), box_position_(box_position) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), box_size_(box_size), box_position_(box_position) { } @@ -191,30 +195,67 @@ bool PickAndPlaceExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - // Create TrajOpt Profile - auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - - auto trajopt_composite_profile = std::make_shared(); - trajopt_composite_profile->longest_valid_segment_length = 0.05; - trajopt_composite_profile->collision_constraint_config.enabled = true; - trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0; - trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; - trajopt_composite_profile->collision_constraint_config.coeff = 10; - trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; - trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; - trajopt_composite_profile->collision_cost_config.coeff = 50; - - auto trajopt_solver_profile = std::make_shared(); - trajopt_solver_profile->opt_info.max_iter = 100; - - auto post_check_profile = std::make_shared(); - // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + if (ifopt_) + { + // 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; + 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_ifopt::CollisionCoeffData(1); + trajopt_ifopt_composite_profile->collision_cost_config->type = + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; + 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_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_accelerations = true; + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_jerks = true; + 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); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + } + else + { + // Create TrajOpt Profile + auto trajopt_plan_profile = std::make_shared(); + trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); + + auto trajopt_composite_profile = std::make_shared(); + trajopt_composite_profile->longest_valid_segment_length = 0.05; + trajopt_composite_profile->collision_constraint_config.enabled = true; + trajopt_composite_profile->collision_constraint_config.safety_margin = 0.0; + trajopt_composite_profile->collision_constraint_config.safety_margin_buffer = 0.005; + trajopt_composite_profile->collision_constraint_config.coeff = 10; + trajopt_composite_profile->collision_cost_config.safety_margin = 0.005; + trajopt_composite_profile->collision_cost_config.safety_margin_buffer = 0.01; + trajopt_composite_profile->collision_cost_config.coeff = 50; + + auto trajopt_solver_profile = std::make_shared(); + trajopt_solver_profile->opt_info.max_iter = 100; + + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + } + + auto post_check_profile = std::make_shared(); profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); @@ -222,7 +263,8 @@ bool PickAndPlaceExample::run() CONSOLE_BRIDGE_logInform("Pick plan"); // Create task - TaskComposerNode::UPtr pick_task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr pick_task = factory.createTaskComposerNode(task_name); const std::string pick_output_key = pick_task->getOutputKeys().front(); // Create Task Composer Problem From 44f526b99683959c4adfedc1866d70440f9e4517 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 10 Oct 2023 13:11:25 +0200 Subject: [PATCH 09/25] Added ifopt option to puzzle_piece_example --- .../tesseract_examples/puzzle_piece_example.h | 5 +- .../src/puzzle_piece_example.cpp | 84 +++++++++++++------ 2 files changed, 63 insertions(+), 26 deletions(-) diff --git a/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h b/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h index 3d72786b39..62ac0eacc7 100644 --- a/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h +++ b/tesseract_examples/include/tesseract_examples/puzzle_piece_example.h @@ -29,7 +29,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -45,7 +44,8 @@ class PuzzlePieceExample : public Example { public: PuzzlePieceExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter = nullptr); + tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false); ~PuzzlePieceExample() override = default; PuzzlePieceExample(const PuzzlePieceExample&) = default; PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default; @@ -55,6 +55,7 @@ class PuzzlePieceExample : public Example bool run() override final; private: + bool ifopt_; static tesseract_common::VectorIsometry3d makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator); }; diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 9d591edc81..78f8cbd2b0 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -42,6 +42,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include #include #include #include @@ -56,6 +58,7 @@ using namespace tesseract_visualization; using namespace tesseract_planning; using tesseract_common::ManipulatorInfo; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; namespace tesseract_examples @@ -123,7 +126,8 @@ PuzzlePieceExample::makePuzzleToolPoses(const tesseract_common::ResourceLocator: } PuzzlePieceExample::PuzzlePieceExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter) + tesseract_visualization::Visualization::Ptr plotter, + bool ifopt) : Example(std::move(env), std::move(plotter)) { } @@ -191,33 +195,65 @@ bool PuzzlePieceExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - // Create TrajOpt Profile - auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); - trajopt_plan_profile->cartesian_coeff(5) = 0; - - auto trajopt_composite_profile = std::make_shared(); - trajopt_composite_profile->collision_constraint_config.enabled = false; - trajopt_composite_profile->collision_cost_config.enabled = true; - trajopt_composite_profile->collision_cost_config.safety_margin = 0.025; - trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP; - trajopt_composite_profile->collision_cost_config.coeff = 20; - - auto trajopt_solver_profile = std::make_shared(); - trajopt_solver_profile->convex_solver = sco::ModelType::OSQP; - trajopt_solver_profile->opt_info.num_threads = 0; - trajopt_solver_profile->opt_info.max_iter = 200; - trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + if (ifopt_) + { + // Create TrajOpt_Ifopt Profile + 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_ifopt::CollisionCoeffData(2); + trajopt_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_accelerations = true; + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + 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); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + } + else + { + // Create TrajOpt Profile + auto trajopt_plan_profile = std::make_shared(); + trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); + trajopt_plan_profile->cartesian_coeff(5) = 0; + + auto trajopt_composite_profile = std::make_shared(); + trajopt_composite_profile->collision_constraint_config.enabled = false; + trajopt_composite_profile->collision_cost_config.enabled = true; + trajopt_composite_profile->collision_cost_config.safety_margin = 0.025; + trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP; + trajopt_composite_profile->collision_cost_config.coeff = 20; + + auto trajopt_solver_profile = std::make_shared(); + trajopt_solver_profile->convex_solver = sco::ModelType::OSQP; + trajopt_solver_profile->opt_info.num_threads = 0; + trajopt_solver_profile->opt_info.max_iter = 200; + trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; + trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + } // Create task - TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().front(); if (plotter_ != nullptr) From 7b2d80240512a39965b3467c16b727d705da260a Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 10 Oct 2023 16:04:20 +0200 Subject: [PATCH 10/25] Fix missing ifopt_ initializer and task_name --- tesseract_examples/src/pick_and_place_example.cpp | 2 +- tesseract_examples/src/puzzle_piece_example.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 36fe3854ec..0f4b6e0e10 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -381,7 +381,7 @@ bool PickAndPlaceExample::run() place_program.print("Program: "); // Create task - TaskComposerNode::UPtr place_task = factory.createTaskComposerNode("TrajOptPipeline"); + TaskComposerNode::UPtr place_task = factory.createTaskComposerNode(task_name); const std::string place_output_key = pick_task->getOutputKeys().front(); // Create Task Composer Problem diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 78f8cbd2b0..f08c9499b3 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -128,7 +128,7 @@ 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)) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) { } From 1d5a9a7e5b53c9ced70601a83c7e4e000ba2fd79 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 13 Oct 2023 14:50:09 +0200 Subject: [PATCH 11/25] Adapt to latest trajopt --- tesseract_examples/src/car_seat_example.cpp | 4 ++-- tesseract_examples/src/pick_and_place_example.cpp | 4 ++-- tesseract_examples/src/puzzle_piece_example.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 31dac95bc5..0a9becb244 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -261,13 +261,13 @@ bool CarSeatExample::run() 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_ifopt::CollisionCoeffData(1); + trajopt_common::CollisionCoeffData(1); trajopt_ifopt_composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; 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_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_common::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 0f4b6e0e10..a5e8370d66 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -207,13 +207,13 @@ bool PickAndPlaceExample::run() 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_ifopt::CollisionCoeffData(1); + trajopt_common::CollisionCoeffData(1); trajopt_ifopt_composite_profile->collision_cost_config->type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; 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_ifopt::CollisionCoeffData(5); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_common::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index f08c9499b3..0035ce632f 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -207,7 +207,7 @@ bool PuzzlePieceExample::run() 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_ifopt::CollisionCoeffData(2); + trajopt_ifopt_composite_profile->collision_cost_config->collision_coeff_data = trajopt_common::CollisionCoeffData(2); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; From ff19472b28521c5e255f13d9e77432f240a38aae Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 28 Nov 2023 19:43:59 +0100 Subject: [PATCH 12/25] clang-format --- tesseract_examples/src/car_seat_example.cpp | 3 ++- tesseract_examples/src/pick_and_place_example.cpp | 3 ++- tesseract_examples/src/puzzle_piece_example.cpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index 0a9becb244..16a335ae85 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -267,7 +267,8 @@ bool CarSeatExample::run() 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_ifopt_composite_profile->collision_cost_config->collision_coeff_data = + trajopt_common::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index a5e8370d66..25ec9836fe 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -213,7 +213,8 @@ bool PickAndPlaceExample::run() 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_ifopt_composite_profile->collision_cost_config->collision_coeff_data = + trajopt_common::CollisionCoeffData(5); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 0035ce632f..f9df6c332e 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -207,7 +207,8 @@ bool PuzzlePieceExample::run() 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_ifopt_composite_profile->collision_cost_config->collision_coeff_data = + trajopt_common::CollisionCoeffData(2); trajopt_ifopt_composite_profile->smooth_velocities = false; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; From 7324cb0bdad1a27de0ddef7f5921aafc78234b58 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 21 Dec 2023 08:01:53 +0100 Subject: [PATCH 13/25] Revert "Add cartesian waypoints to fixed indices" This reverts commit e55888e7cb45b403ffc9d1d7d673c9a606f6573e. --- .../trajopt/src/trajopt_motion_planner.cpp | 4 +--- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 4 +--- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index ec15dc41bc..13905c9d6d 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -273,9 +273,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const seed_states.push_back(request.env_state.getJointValues(joint_names)); } - // Add to fixed indices - if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ - fixed_steps.push_back(i); + /** @todo If fixed cartesian and not term_type cost add as fixed */ } else if (move_instruction.getWaypoint().isJointWaypoint()) { 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 d4b1dd578e..3e533872df 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 @@ -314,9 +314,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Apply profile cur_plan_profile->apply(*problem, cwp, move_instruction, composite_mi, active_links, i); - // Add to fixed indices - if (!cwp.isToleranced()) /** @todo Should not make fixed if term_type is cost */ - fixed_steps.push_back(i); + /** @todo If fixed cartesian and not term_type cost add as fixed */ } else if (move_instruction.getWaypoint().isJointWaypoint()) { From bd9f7d4adc06c20fb7e2137f85d8f9038e0cadef Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 10:05:26 +0100 Subject: [PATCH 14/25] Add Ifopt to puzzle_piece_auxillary_axes example --- .../puzzle_piece_auxillary_axes_example.h | 4 +- .../puzzle_piece_auxillary_axes_example.cpp | 96 +++++++++++++------ .../src/puzzle_piece_example.cpp | 1 - 3 files changed, 69 insertions(+), 32 deletions(-) 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 7a3ed85762..b7866971e8 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 @@ -47,7 +47,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example { public: PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter = nullptr); + tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false); ~PuzzlePieceAuxillaryAxesExample() override = default; PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default; PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default; @@ -57,6 +58,7 @@ class PuzzlePieceAuxillaryAxesExample : public Example bool run() override final; private: + bool ifopt_; static tesseract_common::VectorIsometry3d makePuzzleToolPoses(const tesseract_common::ResourceLocator::ConstPtr& locator); }; diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 2126b982d9..e9f540a501 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -41,6 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include #include #include #include @@ -54,8 +56,8 @@ using namespace tesseract_scene_graph; using namespace tesseract_collision; using namespace tesseract_visualization; using namespace tesseract_planning; -using tesseract_common::ManipulatorInfo; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; namespace tesseract_examples @@ -123,8 +125,9 @@ PuzzlePieceAuxillaryAxesExample::makePuzzleToolPoses(const tesseract_common::Res } PuzzlePieceAuxillaryAxesExample::PuzzlePieceAuxillaryAxesExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter) - : Example(std::move(env), std::move(plotter)) + tesseract_visualization::Visualization::Ptr plotter, + bool ifopt) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt) { } @@ -195,37 +198,70 @@ bool PuzzlePieceAuxillaryAxesExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); - // Create TrajOpt Profile - auto trajopt_plan_profile = std::make_shared(); - trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); - trajopt_plan_profile->cartesian_coeff(3) = 2; - trajopt_plan_profile->cartesian_coeff(4) = 2; - trajopt_plan_profile->cartesian_coeff(5) = 0; - - auto trajopt_composite_profile = std::make_shared(); - trajopt_composite_profile->collision_constraint_config.enabled = false; - trajopt_composite_profile->collision_cost_config.enabled = true; - trajopt_composite_profile->collision_cost_config.safety_margin = 0.025; - trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP; - trajopt_composite_profile->collision_cost_config.coeff = 1; - - auto trajopt_solver_profile = std::make_shared(); - trajopt_solver_profile->convex_solver = sco::ModelType::OSQP; - auto convex_solver_config = std::make_shared(); - convex_solver_config->settings.adaptive_rho = 0; - trajopt_solver_profile->convex_solver_config = convex_solver_config; - trajopt_solver_profile->opt_info.max_iter = 200; - trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; - trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + if (ifopt_) + { + // Create TrajOpt_Ifopt Profile + 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_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->smooth_accelerations = true; + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + 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); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + } + else + { + // Create TrajOpt Profile + auto trajopt_plan_profile = std::make_shared(); + trajopt_plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); + trajopt_plan_profile->cartesian_coeff(3) = 2; + trajopt_plan_profile->cartesian_coeff(4) = 2; + trajopt_plan_profile->cartesian_coeff(5) = 0; + + auto trajopt_composite_profile = std::make_shared(); + trajopt_composite_profile->collision_constraint_config.enabled = false; + trajopt_composite_profile->collision_cost_config.enabled = true; + trajopt_composite_profile->collision_cost_config.safety_margin = 0.025; + trajopt_composite_profile->collision_cost_config.type = trajopt::CollisionEvaluatorType::SINGLE_TIMESTEP; + trajopt_composite_profile->collision_cost_config.coeff = 1; + + auto trajopt_solver_profile = std::make_shared(); + trajopt_solver_profile->convex_solver = sco::ModelType::OSQP; + auto convex_solver_config = std::make_shared(); + convex_solver_config->settings.adaptive_rho = 0; + trajopt_solver_profile->convex_solver_config = convex_solver_config; + trajopt_solver_profile->opt_info.max_iter = 200; + trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; + trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; + + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + } // Create task - TaskComposerNode::UPtr task = factory.createTaskComposerNode("TrajOptPipeline"); + const std::string task_name = (ifopt_) ? "TrajOptIfoptPipeline" : "TrajOptPipeline"; + TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().front(); if (plotter_ != nullptr) diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index f9df6c332e..9c8f6c120c 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -56,7 +56,6 @@ using namespace tesseract_scene_graph; using namespace tesseract_collision; using namespace tesseract_visualization; using namespace tesseract_planning; -using tesseract_common::ManipulatorInfo; static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; static const std::string TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"; From fb0c5a630b831822038dbd2df0c07b58d375cde7 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 11:22:19 +0100 Subject: [PATCH 15/25] Add FreespaceIfoptPipeline --- .../config/task_composer_plugins.yaml | 78 +++++++++++++++++++ .../config/task_composer_plugins.yaml.schema | 4 + 2 files changed, 82 insertions(+) diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index aa4b8a2fa0..a7c67c4b19 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml +++ b/tesseract_task_composer/config/task_composer_plugins.yaml @@ -639,6 +639,84 @@ task_composer_plugins: - source: ProcessInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] + FreespaceIfoptTask: + class: PipelineTaskFactory + config: + conditional: true + inputs: [input_data] + outputs: [output_data] + nodes: + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + MinLengthTask: + class: MinLengthTaskFactory + config: + conditional: true + inputs: [input_data] + outputs: [output_data] + OMPLMotionPlannerTask: + class: OMPLMotionPlannerTaskFactory + config: + conditional: true + inputs: [output_data] + outputs: [output_data] + format_result_as_input: true + TrajOptIfoptMotionPlannerTask: + class: TrajOptIfoptMotionPlannerTaskFactory + config: + conditional: true + inputs: [output_data] + outputs: [output_data] + format_result_as_input: false + DiscreteContactCheckTask: + class: DiscreteContactCheckTaskFactory + config: + conditional: true + inputs: [output_data] + IterativeSplineParameterizationTask: + class: IterativeSplineParameterizationTaskFactory + config: + conditional: true + inputs: [output_data] + outputs: [output_data] + edges: + - source: MinLengthTask + destinations: [ErrorTask, OMPLMotionPlannerTask] + - source: OMPLMotionPlannerTask + destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask] + - source: TrajOptIfoptMotionPlannerTask + destinations: [ErrorTask, DiscreteContactCheckTask] + - source: DiscreteContactCheckTask + destinations: [ErrorTask, IterativeSplineParameterizationTask] + - source: IterativeSplineParameterizationTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] + FreespaceIfoptPipeline: + class: PipelineTaskFactory + config: + conditional: false + outputs: [output_data] + nodes: + ProcessInputTask: + class: ProcessPlanningInputTaskFactory + config: + conditional: false + outputs: [input_data] + MotionPlanningTask: + task: FreespaceIfoptTask + config: + conditional: false + abort_terminal: 0 + edges: + - source: ProcessInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] RasterFtTask: class: PipelineTaskFactory config: diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml.schema b/tesseract_task_composer/config/task_composer_plugins.yaml.schema index 6080974991..7f1f9b1853 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml.schema +++ b/tesseract_task_composer/config/task_composer_plugins.yaml.schema @@ -124,6 +124,10 @@ definitions: "$ref": "#/definitions/CartesianTaskClass" FreespacePipeline: "$ref": "#/definitions/Pipeline" + FreespaceIfoptTask: + "$ref": "#/definitions/CartesianTaskClass" + FreespaceIfoptPipeline: + "$ref": "#/definitions/Pipeline" RasterFtTask: "$ref": "#/definitions/RasterTask" RasterFtPipeline: From 24cd74a9b094fdbd830384d5b0aa760be6bb537c Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 11:22:37 +0100 Subject: [PATCH 16/25] Add Ifopt to freespace_hybrid_example --- .../freespace_hybrid_example.h | 7 ++-- .../src/freespace_hybrid_example.cpp | 37 +++++++++++++++++-- 2 files changed, 37 insertions(+), 7 deletions(-) diff --git a/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h b/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h index f6abd31490..3a15fa50c5 100644 --- a/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h +++ b/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h @@ -27,9 +27,6 @@ #define TESSERACT_EXAMPLES_FREESPACE_HYBRID_EXAMPLE_H #include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -44,6 +41,8 @@ class FreespaceHybridExample : public Example public: FreespaceHybridExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter = nullptr, + bool ifopt = false, + bool debug = false, double range = 0.01, double planning_time = 60.0); ~FreespaceHybridExample() override = default; @@ -55,6 +54,8 @@ class FreespaceHybridExample : public Example bool run() override final; private: + bool ifopt_; + bool debug_; double range_; double planning_time_; diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 3b40c87e8c..9ddcf3cb97 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -56,14 +57,21 @@ using namespace tesseract_planning; using tesseract_common::ManipulatorInfo; static const std::string OMPL_DEFAULT_NAMESPACE = "OMPLMotionPlannerTask"; +static const std::string TRAJOPT_IFOPT_DEFAULT_NAMESPACE = "TrajOptIfoptMotionPlannerTask"; namespace tesseract_examples { FreespaceHybridExample::FreespaceHybridExample(tesseract_environment::Environment::Ptr env, tesseract_visualization::Visualization::Ptr plotter, + bool ifopt, + bool debug, double range, double planning_time) - : Example(std::move(env), std::move(plotter)), range_(range), planning_time_(planning_time) + : Example(std::move(env), std::move(plotter)) + , ifopt_(ifopt) + , debug_(debug) + , range_(range) + , planning_time_(planning_time) { } @@ -93,6 +101,9 @@ Command::Ptr FreespaceHybridExample::addSphere() bool FreespaceHybridExample::run() { + if (debug_) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + // Add sphere to environment Command::Ptr cmd = addSphere(); if (!env_->applyCommand(cmd)) @@ -163,6 +174,9 @@ bool FreespaceHybridExample::run() // Create Executor auto executor = factory.createTaskComposerExecutor("TaskflowExecutor"); + // Create profile dictionary + auto profiles = std::make_shared(); + // Create OMPL Profile auto ompl_profile = std::make_shared(); auto ompl_planner_config = std::make_shared(); @@ -170,12 +184,27 @@ bool FreespaceHybridExample::run() ompl_profile->planning_time = planning_time_; ompl_profile->planners = { ompl_planner_config, ompl_planner_config }; - // Create profile dictionary - auto profiles = std::make_shared(); profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + if (ifopt_) + { + // Create TrajOpt_Ifopt Profile + auto trajopt_ifopt_composite_profile = std::make_shared(); + trajopt_ifopt_composite_profile->collision_constraint_config->contact_manager_config = + tesseract_collision::ContactManagerConfig(0.025); + trajopt_ifopt_composite_profile->collision_cost_config->contact_manager_config = + tesseract_collision::ContactManagerConfig(0.025); + trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); + trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); + + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + } + // Create task - TaskComposerNode::UPtr task = factory.createTaskComposerNode("FreespacePipeline"); + const std::string task_name = (ifopt_) ? "FreespaceIfoptPipeline" : "FreespacePipeline"; + TaskComposerNode::UPtr task = factory.createTaskComposerNode(task_name); const std::string output_key = task->getOutputKeys().front(); // Create Task Composer Problem From 1fbec978de27b597c54559a3fab130e6d939fdcc Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 15:48:54 +0100 Subject: [PATCH 17/25] - 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 { From f826669a57a1e567d09f50a4ccc591f3941a130f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 16:37:31 +0100 Subject: [PATCH 18/25] Revert "- Added convex_solver_settings with proper defaults to TrajOptIfoptDefaultSolverProfile" This reverts commit 0242aa876ec34b2e48d86c04ed935b19c18a9aa6. --- .../trajopt_ifopt_default_solver_profile.h | 15 ++---- .../trajopt_ifopt/trajopt_ifopt_problem.h | 10 ---- .../trajopt_ifopt_default_solver_profile.cpp | 14 ------ .../src/trajopt_ifopt_motion_planner.cpp | 47 +++++-------------- 4 files changed, 14 insertions(+), 72 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 bd81351e0e..4675998603 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 @@ -26,11 +26,6 @@ #ifndef TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H #define TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H -#include -TRAJOPT_IGNORE_WARNINGS_PUSH -#include -TRAJOPT_IGNORE_WARNINGS_POP - #include #include @@ -43,19 +38,15 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptDefaultSolverProfile(); + TrajOptIfoptDefaultSolverProfile() = default; ~TrajOptIfoptDefaultSolverProfile() override = default; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = default; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = default; 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{}; - - /** @brief Optimization parameters */ - trajopt_sqp::SQPParameters opt_info{}; + /** @brief Optimization paramters */ + 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 da32e20b27..84113c103c 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,12 +30,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include -#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include namespace tesseract_planning @@ -53,8 +51,6 @@ 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 @@ -66,12 +62,6 @@ 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; - trajopt_sqp::QPProblem::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 de6646961b..3aa9ef80b8 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 @@ -28,22 +28,8 @@ 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 3e533872df..302c0daa76 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 @@ -63,34 +63,6 @@ MotionPlanner::Ptr TrajOptIfoptMotionPlanner::clone() const return std::make_shared(name_); } -void OSQPEigenSolver_setSettings(const OsqpEigen::Solver& 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; @@ -125,14 +97,17 @@ PlannerResponse TrajOptIfoptMotionPlanner::solve(const PlannerRequest& request) // Create optimizer /** @todo Enable solver selection (e.g. IPOPT) */ - problem->qp_solver = std::make_shared(); - OSQPEigenSolver_setSettings(dynamic_cast(*problem->qp_solver).solver_, - problem->convex_solver_settings); - dynamic_cast(*problem->qp_solver) - .solver_.settings() - ->setVerbosity((problem->convex_solver_settings.verbose != 0) || request.verbose); - - trajopt_sqp::TrustRegionSQPSolver solver(problem->qp_solver); + 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); + solver.params = problem->opt_info; // Add all callbacks From 8973532c6e8413ccb65341d04bd3b8ae5c9592b0 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Tue, 9 Jan 2024 16:44:55 +0100 Subject: [PATCH 19/25] Fix for revert of convex_solver_settings --- tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index bdee176237..ccb49fd73e 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -232,7 +232,7 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); auto trajopt_ifopt_solver_profile = std::make_shared(); - trajopt_ifopt_solver_profile->convex_solver_settings.adaptive_rho = 0; + // 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; From 4a3f57a7bf9e2b6dbbe8ae776504cbf45f7a2eef Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 10 Jan 2024 09:33:40 +0100 Subject: [PATCH 20/25] Added unit tests for ifopt examples --- .../src/pick_and_place_example.cpp | 10 ++-- .../test/car_seat_example_unit.cpp | 19 +++++++- .../test/freespace_hybrid_example_unit.cpp | 48 +++++++++++++++++++ .../test/pick_and_place_example_unit.cpp | 19 +++++++- ...zzle_piece_auxillary_axes_example_unit.cpp | 19 +++++++- .../test/puzzle_piece_example_unit.cpp | 19 +++++++- 6 files changed, 121 insertions(+), 13 deletions(-) create mode 100644 tesseract_examples/test/freespace_hybrid_example_unit.cpp diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index a29bd82747..26fdf0d8ee 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -210,24 +210,24 @@ bool PickAndPlaceExample::run() { // 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->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 10); 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_CONTINUOUS; + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; 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_common::CollisionCoeffData(10); trajopt_ifopt_composite_profile->collision_cost_config->type = - tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; + tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; 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; diff --git a/tesseract_examples/test/car_seat_example_unit.cpp b/tesseract_examples/test/car_seat_example_unit.cpp index 50b55d1981..d72287d311 100644 --- a/tesseract_examples/test/car_seat_example_unit.cpp +++ b/tesseract_examples/test/car_seat_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, CarSeatCppExampleUnit) // NOLINT +TEST(TesseractExamples, CarSeatCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, CarSeatCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - CarSeatExample example(env, nullptr); + CarSeatExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, CarSeatCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/car_seat_demo.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/car_seat_demo.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + CarSeatExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/freespace_hybrid_example_unit.cpp b/tesseract_examples/test/freespace_hybrid_example_unit.cpp new file mode 100644 index 0000000000..88317ffea2 --- /dev/null +++ b/tesseract_examples/test/freespace_hybrid_example_unit.cpp @@ -0,0 +1,48 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +using namespace tesseract_examples; +using namespace tesseract_common; +using namespace tesseract_environment; + +TEST(TesseractExamples, FreespaceHybridTrajOptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + FreespaceHybridExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, FreespaceHybridTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + FreespaceHybridExample example(env, nullptr, true, false); + EXPECT_TRUE(example.run()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp index ea7ba8e3c2..e7f2d9662d 100644 --- a/tesseract_examples/test/pick_and_place_example_unit.cpp +++ b/tesseract_examples/test/pick_and_place_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, PickAndPlaceCppExampleUnit) // NOLINT +TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, PickAndPlaceCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PickAndPlaceExample example(env, nullptr); + PickAndPlaceExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/pick_and_place_plan.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PickAndPlaceExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp index 44c33a1396..cd12f6becb 100644 --- a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppExampleUnit) // NOLINT +TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppExampleUnit) // NOL if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PuzzlePieceAuxillaryAxesExample example(env, nullptr); + PuzzlePieceAuxillaryAxesExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PuzzlePieceAuxillaryAxesExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } diff --git a/tesseract_examples/test/puzzle_piece_example_unit.cpp b/tesseract_examples/test/puzzle_piece_example_unit.cpp index 09a1dc6756..978fa6691e 100644 --- a/tesseract_examples/test/puzzle_piece_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, PuzzlePieceCppExampleUnit) // NOLINT +TEST(TesseractExamples, PuzzlePieceCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = @@ -21,7 +21,22 @@ TEST(TesseractExamples, PuzzlePieceCppExampleUnit) // NOLINT if (!env->init(urdf_path, srdf_path, locator)) exit(1); - PuzzlePieceExample example(env, nullptr); + PuzzlePieceExample example(env, nullptr, false, false); + EXPECT_TRUE(example.run()); +} + +TEST(TesseractExamples, DISABLED_PuzzlePieceCppTrajOptIfoptExampleUnit) // NOLINT +{ + auto locator = std::make_shared(); + tesseract_common::fs::path urdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.urdf")->getFilePath(); + tesseract_common::fs::path srdf_path = + locator->locateResource("package://tesseract_support/urdf/puzzle_piece_workcell.srdf")->getFilePath(); + auto env = std::make_shared(); + if (!env->init(urdf_path, srdf_path, locator)) + exit(1); + + PuzzlePieceExample example(env, nullptr, true, false); EXPECT_TRUE(example.run()); } From c9fccfcb1cef5ba5fe20547dc71d9f0d0363671f Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 10 Jan 2024 12:50:49 +0100 Subject: [PATCH 21/25] Disable pick_and_place_example TrajoptIfopt unit test --- tesseract_examples/test/pick_and_place_example_unit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp index e7f2d9662d..be8f593720 100644 --- a/tesseract_examples/test/pick_and_place_example_unit.cpp +++ b/tesseract_examples/test/pick_and_place_example_unit.cpp @@ -25,7 +25,7 @@ TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT EXPECT_TRUE(example.run()); } -TEST(TesseractExamples, PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT +TEST(TesseractExamples, DISABLED_PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = From debf109516a09e90562db550b397c6e05f959507 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 17 Jan 2024 20:20:47 +0100 Subject: [PATCH 22/25] Update number of plugins in unit test --- .../test/tesseract_task_composer_plugin_factories_unit.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp index b0202c0a77..4188f5eb2e 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_plugin_factories_unit.cpp @@ -72,7 +72,7 @@ void runTaskComposerFactoryTest(TaskComposerPluginFactory& factory, YAML::Node p EXPECT_TRUE(cm != nullptr); } #ifdef TESSERACT_TASK_COMPOSER_HAS_TRAJOPT_IFOPT - EXPECT_EQ(task_plugins.size(), 34); + EXPECT_EQ(task_plugins.size(), 36); #else EXPECT_EQ(task_plugins.size(), 32); #endif From e4d9ff3495a3f29f9122395dc3105c0bedb31d9d Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Fri, 16 Feb 2024 20:27:27 +0100 Subject: [PATCH 23/25] Enabled remaining ifopt example unit tests and enabled puzzle piece auxillary axis example unit test --- tesseract_examples/test/glass_upright_example_unit.cpp | 2 +- tesseract_examples/test/pick_and_place_example_unit.cpp | 2 +- .../test/puzzle_piece_auxillary_axes_example_unit.cpp | 2 +- tesseract_examples/test/puzzle_piece_example_unit.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/tesseract_examples/test/glass_upright_example_unit.cpp b/tesseract_examples/test/glass_upright_example_unit.cpp index 521ffe8038..7414756bfd 100644 --- a/tesseract_examples/test/glass_upright_example_unit.cpp +++ b/tesseract_examples/test/glass_upright_example_unit.cpp @@ -25,7 +25,7 @@ TEST(TesseractExamples, GlassUprightTrajOptExampleUnit) // NOLINT EXPECT_TRUE(example.run()); } -TEST(TesseractExamples, DISABLED_GlassUprightTrajOptIfoptExampleUnit) // NOLINT +TEST(TesseractExamples, GlassUprightTrajOptIfoptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = diff --git a/tesseract_examples/test/pick_and_place_example_unit.cpp b/tesseract_examples/test/pick_and_place_example_unit.cpp index be8f593720..e7f2d9662d 100644 --- a/tesseract_examples/test/pick_and_place_example_unit.cpp +++ b/tesseract_examples/test/pick_and_place_example_unit.cpp @@ -25,7 +25,7 @@ TEST(TesseractExamples, PickAndPlaceCppTrajOptExampleUnit) // NOLINT EXPECT_TRUE(example.run()); } -TEST(TesseractExamples, DISABLED_PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT +TEST(TesseractExamples, PickAndPlaceCppTrajOptIfoptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = diff --git a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp index cd12f6becb..e99a450a5e 100644 --- a/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_auxillary_axes_example_unit.cpp @@ -10,7 +10,7 @@ using namespace tesseract_examples; using namespace tesseract_common; using namespace tesseract_environment; -TEST(TesseractExamples, DISABLED_PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLINT +TEST(TesseractExamples, PuzzlePieceAuxillaryAxesCppTrajOptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = diff --git a/tesseract_examples/test/puzzle_piece_example_unit.cpp b/tesseract_examples/test/puzzle_piece_example_unit.cpp index 978fa6691e..fe125a8ebc 100644 --- a/tesseract_examples/test/puzzle_piece_example_unit.cpp +++ b/tesseract_examples/test/puzzle_piece_example_unit.cpp @@ -25,7 +25,7 @@ TEST(TesseractExamples, PuzzlePieceCppTrajOptExampleUnit) // NOLINT EXPECT_TRUE(example.run()); } -TEST(TesseractExamples, DISABLED_PuzzlePieceCppTrajOptIfoptExampleUnit) // NOLINT +TEST(TesseractExamples, PuzzlePieceCppTrajOptIfoptExampleUnit) // NOLINT { auto locator = std::make_shared(); tesseract_common::fs::path urdf_path = From 6789153f6bb8077958db00b366fea5795fd34164 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 16 Feb 2024 22:45:58 -0600 Subject: [PATCH 24/25] Fix pick and place ifopt example version --- tesseract_examples/src/pick_and_place_example.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 26fdf0d8ee..b052b86a25 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -228,7 +228,7 @@ bool PickAndPlaceExample::run() 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(50); - trajopt_ifopt_composite_profile->smooth_velocities = false; + trajopt_ifopt_composite_profile->smooth_velocities = true; trajopt_ifopt_composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->smooth_accelerations = true; trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); From 97fb92586d4e5bc5b944ada464043a52ce181956 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 17 Feb 2024 08:54:56 -0600 Subject: [PATCH 25/25] Update Unstable CI names --- .github/workflows/unstable.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/unstable.yml b/.github/workflows/unstable.yml index 9461f966c4..291fb1178a 100644 --- a/.github/workflows/unstable.yml +++ b/.github/workflows/unstable.yml @@ -18,7 +18,7 @@ on: jobs: ci: - name: ${{ matrix.distro }} + name: ${{ matrix.distro }}-unstable runs-on: ubuntu-latest strategy: fail-fast: false