From c791e7488de19c4f3a5dfc2eccf2624e415aa427 Mon Sep 17 00:00:00 2001 From: Roelof Date: Sat, 17 Feb 2024 18:54:31 +0100 Subject: [PATCH] Adding Trajopt_Ifopt option to all examples (#389) --- .clang-format | 1 - .github/workflows/unstable.yml | 2 +- .../tesseract_examples/car_seat_example.h | 7 +- .../freespace_hybrid_example.h | 7 +- .../pick_and_place_example.h | 4 + .../puzzle_piece_auxillary_axes_example.h | 6 +- .../tesseract_examples/puzzle_piece_example.h | 7 +- .../src/basic_cartesian_example.cpp | 2 + tesseract_examples/src/car_seat_example.cpp | 100 ++++++++++++---- .../src/car_seat_example_node.cpp | 2 +- .../src/freespace_hybrid_example.cpp | 39 +++++- .../src/glass_upright_example.cpp | 2 + .../src/pick_and_place_example.cpp | 109 +++++++++++++---- .../puzzle_piece_auxillary_axes_example.cpp | 113 +++++++++++++----- .../src/puzzle_piece_example.cpp | 101 +++++++++++----- .../test/car_seat_example_unit.cpp | 19 ++- .../test/freespace_hybrid_example_unit.cpp | 48 ++++++++ .../test/glass_upright_example_unit.cpp | 2 +- .../test/pick_and_place_example_unit.cpp | 19 ++- ...zzle_piece_auxillary_axes_example_unit.cpp | 19 ++- .../test/puzzle_piece_example_unit.cpp | 19 ++- .../config/task_composer_plugins.yaml | 78 ++++++++++++ .../config/task_composer_plugins.yaml.schema | 4 + ...ct_task_composer_plugin_factories_unit.cpp | 2 +- 24 files changed, 581 insertions(+), 131 deletions(-) create mode 100644 tesseract_examples/test/freespace_hybrid_example_unit.cpp diff --git a/.clang-format b/.clang-format index 8b3d0c00dab..792fd96a05f 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/.github/workflows/unstable.yml b/.github/workflows/unstable.yml index 9461f966c46..291fb1178a0 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 diff --git a/tesseract_examples/include/tesseract_examples/car_seat_example.h b/tesseract_examples/include/tesseract_examples/car_seat_example.h index 5a9fb8e5bd6..2ea0332de7d 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,9 @@ 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, + bool debug = false); ~CarSeatExample() override = default; CarSeatExample(const CarSeatExample&) = default; CarSeatExample& operator=(const CarSeatExample&) = default; @@ -54,6 +55,8 @@ class CarSeatExample : public Example bool run() override final; private: + bool ifopt_; + bool debug_; std::unordered_map> saved_positions_; static std::unordered_map> getPredefinedPosition(); diff --git a/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h b/tesseract_examples/include/tesseract_examples/freespace_hybrid_example.h index f6abd314903..3a15fa50c53 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/include/tesseract_examples/pick_and_place_example.h b/tesseract_examples/include/tesseract_examples/pick_and_place_example.h index 1b7e84453e4..0270c429240 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,8 @@ class PickAndPlaceExample : public Example public: 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; @@ -56,6 +58,8 @@ class PickAndPlaceExample : public Example bool run() override final; 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 7a3ed857622..ad869bd0b83 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,9 @@ 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, + bool debug = false); ~PuzzlePieceAuxillaryAxesExample() override = default; PuzzlePieceAuxillaryAxesExample(const PuzzlePieceAuxillaryAxesExample&) = default; PuzzlePieceAuxillaryAxesExample& operator=(const PuzzlePieceAuxillaryAxesExample&) = default; @@ -57,6 +59,8 @@ class PuzzlePieceAuxillaryAxesExample : public Example bool run() override final; 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 3d72786b391..8a327fb6b9c 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,9 @@ 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, + bool debug = false); ~PuzzlePieceExample() override = default; PuzzlePieceExample(const PuzzlePieceExample&) = default; PuzzlePieceExample& operator=(const PuzzlePieceExample&) = default; @@ -55,6 +56,8 @@ class PuzzlePieceExample : public Example bool run() override final; 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 5fe07d2b457..713ce6fd9ad 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 825b728748b..611f1514332 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -47,6 +47,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include +#include #include #include @@ -58,6 +61,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 +116,10 @@ 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, + bool debug) + : Example(std::move(env), std::move(plotter)), ifopt_(ifopt), debug_(debug) { } @@ -215,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); @@ -245,25 +254,70 @@ 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_Ifopt Profile + auto trajopt_ifopt_composite_profile = std::make_shared(); + trajopt_ifopt_composite_profile->collision_constraint_config->type = + 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_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(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; + 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 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); + + 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 + { + // 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 +351,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 @@ -381,7 +436,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 59405b4db22..9280711ec0d 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); } diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index 3b40c87e8c5..9a15d112b99 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,11 @@ Command::Ptr FreespaceHybridExample::addSphere() 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(); if (!env_->applyCommand(cmd)) @@ -163,6 +176,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 +186,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 diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index 83be6b007fb..213e80cfdd8 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 c37d8683597..b052b86a25d 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -34,6 +34,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include #include #include #include @@ -63,14 +66,21 @@ 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, + bool debug, 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) + , debug_(debug) + , box_size_(box_size) + , box_position_(box_position) { } @@ -104,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); @@ -191,30 +204,73 @@ 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_plan_profile = std::make_shared(); + 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_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(10); + 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_common::CollisionCoeffData(50); + 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); + 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_solver_profile = std::make_shared(); + trajopt_ifopt_solver_profile->opt_info.max_iterations = 100; + + 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 + { + // 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 +278,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 @@ -339,7 +396,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_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 2126b982d95..ccb49fd73e8 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -41,6 +41,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include #include #include #include @@ -54,8 +57,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,14 +126,19 @@ 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, + 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(); @@ -195,37 +203,80 @@ 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_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 = + 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_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, "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 + { + // 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 9d591edc81a..b1020c0ef83 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -42,6 +42,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include +#include #include #include #include @@ -54,8 +57,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,14 +126,19 @@ PuzzlePieceExample::makePuzzleToolPoses(const tesseract_common::ResourceLocator: } PuzzlePieceExample::PuzzlePieceExample(tesseract_environment::Environment::Ptr env, - tesseract_visualization::Visualization::Ptr plotter) - : Example(std::move(env), std::move(plotter)) + tesseract_visualization::Visualization::Ptr plotter, + 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(); @@ -191,33 +199,72 @@ 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_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_coeff_data = + 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; + 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_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, "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 + { + // 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) diff --git a/tesseract_examples/test/car_seat_example_unit.cpp b/tesseract_examples/test/car_seat_example_unit.cpp index 50b55d19818..d72287d3119 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 00000000000..88317ffea28 --- /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/glass_upright_example_unit.cpp b/tesseract_examples/test/glass_upright_example_unit.cpp index 521ffe8038a..7414756bfd3 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 ea7ba8e3c2d..e7f2d9662dd 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 44c33a13966..e99a450a5e3 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, 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 09a1dc67569..fe125a8ebc4 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, 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()); } diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index aa4b8a2fa0e..a7c67c4b190 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 60809749916..7f1f9b1853f 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: 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 b0202c0a77f..4188f5eb2e6 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