From 519851f0d3653dabb4fa17e68a5d8bb198747fdf Mon Sep 17 00:00:00 2001 From: Levi-Armstrong Date: Wed, 3 Nov 2021 10:49:38 -0500 Subject: [PATCH] Update taskflows to leverage graph taskflow --- tesseract_process_managers/CMakeLists.txt | 10 +- .../examples/taskflow_profiling_example.cpp | 15 +- .../core/default_process_planners.h | 31 +- .../core/task_input.h | 14 +- .../check_input_task_generator.h | 79 +++ .../task_generators/has_seed_task_generator.h | 61 ++ .../taskflow_generators/cartesian_taskflow.h | 77 --- .../taskflow_generators/descartes_taskflow.h | 77 --- .../taskflow_generators/freespace_taskflow.h | 84 --- .../taskflow_generators/graph_taskflow.h | 2 +- .../taskflow_generators/ompl_taskflow.h | 76 --- .../trajopt_ifopt_taskflow.h | 77 --- .../taskflow_generators/trajopt_taskflow.h | 77 --- .../utils/taskflow_visualization_utils.h | 4 +- .../src/core/default_process_planners.cpp | 556 ++++++++++++++---- .../check_input_task_generator.cpp | 80 +++ .../has_seed_task_generator.cpp | 51 ++ .../cartesian_taskflow.cpp | 190 ------ .../descartes_taskflow.cpp | 168 ------ .../freespace_taskflow.cpp | 251 -------- .../src/taskflow_generators/ompl_taskflow.cpp | 166 ------ .../trajopt_ifopt_taskflow.cpp | 181 ------ .../taskflow_generators/trajopt_taskflow.cpp | 179 ------ .../test/tesseract_process_managers_unit.cpp | 4 - 24 files changed, 754 insertions(+), 1756 deletions(-) create mode 100644 tesseract_process_managers/include/tesseract_process_managers/task_generators/check_input_task_generator.h create mode 100644 tesseract_process_managers/include/tesseract_process_managers/task_generators/has_seed_task_generator.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_ifopt_taskflow.h delete mode 100644 tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h create mode 100644 tesseract_process_managers/src/task_generators/check_input_task_generator.cpp create mode 100644 tesseract_process_managers/src/task_generators/has_seed_task_generator.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/trajopt_ifopt_taskflow.cpp delete mode 100644 tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp diff --git a/tesseract_process_managers/CMakeLists.txt b/tesseract_process_managers/CMakeLists.txt index 687dcc88b8a..762edf4ac01 100644 --- a/tesseract_process_managers/CMakeLists.txt +++ b/tesseract_process_managers/CMakeLists.txt @@ -59,10 +59,12 @@ add_library( src/core/default_process_planners.cpp src/core/taskflow_container.cpp src/core/utils.cpp + src/task_generators/check_input_task_generator.cpp src/task_generators/continuous_contact_check_task_generator.cpp src/task_generators/discrete_contact_check_task_generator.cpp src/task_generators/fix_state_bounds_task_generator.cpp src/task_generators/fix_state_collision_task_generator.cpp + src/task_generators/has_seed_task_generator.cpp src/task_generators/iterative_spline_parameterization_task_generator.cpp src/task_generators/motion_planner_task_generator.cpp src/task_generators/profile_switch_task_generator.cpp @@ -75,13 +77,7 @@ add_library( src/taskflow_generators/raster_only_global_taskflow.cpp src/taskflow_generators/raster_dt_taskflow.cpp src/taskflow_generators/raster_waad_taskflow.cpp - src/taskflow_generators/raster_waad_dt_taskflow.cpp - src/taskflow_generators/descartes_taskflow.cpp - src/taskflow_generators/ompl_taskflow.cpp - src/taskflow_generators/trajopt_ifopt_taskflow.cpp - src/taskflow_generators/trajopt_taskflow.cpp - src/taskflow_generators/cartesian_taskflow.cpp - src/taskflow_generators/freespace_taskflow.cpp) + src/taskflow_generators/raster_waad_dt_taskflow.cpp) target_link_libraries( ${PROJECT_NAME} PUBLIC console_bridge::console_bridge diff --git a/tesseract_process_managers/examples/taskflow_profiling_example.cpp b/tesseract_process_managers/examples/taskflow_profiling_example.cpp index 540fc5e934d..97d20fecc45 100644 --- a/tesseract_process_managers/examples/taskflow_profiling_example.cpp +++ b/tesseract_process_managers/examples/taskflow_profiling_example.cpp @@ -40,9 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include #include +#include #include using namespace tesseract_planning; @@ -149,13 +149,12 @@ int main() auto display_info = profiler.getTaskDisplayInfo(); /// @todo When FreespaceTaskflow is rewritten to inherit from GraphTaskflow this will work. - // std::ofstream out_data; - // out_data.open(tesseract_common::getTempPath() + "profiling_taskflow-" + tesseract_common::getTimestampString() + - // ".dot"); - // FreespaceTaskflowParams params; - // auto graph_taskflow = std::make_unique(params); - // dump(out_data, graph_taskflow, display_info); - // out_data.close(); + std::ofstream out_data; + out_data.open(tesseract_common::getTempPath() + "profiling_taskflow-" + tesseract_common::getTimestampString() + + ".dot"); + auto graph_taskflow = createFreespaceGenerator(); + dump(out_data, dynamic_cast(*graph_taskflow), display_info); + out_data.close(); return 0; } diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h b/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h index 2a3959531ca..4c9ff77d940 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/default_process_planners.h @@ -31,22 +31,41 @@ namespace tesseract_planning { /** @brief Create TrajOpt Process Pipeline */ -TaskflowGenerator::UPtr createTrajOptGenerator(); +TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input = true, bool post_collisin_check = true); /** @brief Create TrajOpt IFOPT Process Pipeline */ -TaskflowGenerator::UPtr createTrajOptIfoptGenerator(); +TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input = true, bool post_collisin_check = true); /** @brief Create OMPL Process Pipeline */ -TaskflowGenerator::UPtr createOMPLGenerator(); +TaskflowGenerator::UPtr createOMPLGenerator(bool check_input = true, bool post_collisin_check = true); /** @brief Create Descartes Process Pipeline */ -TaskflowGenerator::UPtr createDescartesGenerator(); +TaskflowGenerator::UPtr createDescartesGenerator(bool check_input = true, bool post_collisin_check = true); + +/** + * @brief Create Descartes Only Process Pipeline + * @details This does not perform post collision check or time parameterization + */ +TaskflowGenerator::UPtr createDescartesOnlyGenerator(bool check_input = true); + +/** + * @brief Create Descartes No Collision Check Process Pipeline + * @details This does not perform post collision check + */ +TaskflowGenerator::UPtr createDescartesNoCollisionGenerator(bool check_input = true); /** @brief Create Cartesian Process Pipeline */ -TaskflowGenerator::UPtr createCartesianGenerator(); +TaskflowGenerator::UPtr createCartesianGenerator(bool check_input = true); /** @brief Create Freespace Process Pipeline */ -TaskflowGenerator::UPtr createFreespaceGenerator(); +TaskflowGenerator::UPtr createFreespaceGenerator(bool check_input = true); + +/** + * @brief Create Freespace TrajOpt First Process Pipeline + * @details This will try trajopt first before OMPL, but if trajopt fails + * it runs OMPL followed by trajopt. + */ +TaskflowGenerator::UPtr createFreespaceTrajOptFirstGenerator(bool check_input = true); /** @brief Create Raster Process Pipeline */ TaskflowGenerator::UPtr createRasterGenerator(); diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h b/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h index 2b8fd34ec90..e39440a978f 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/task_input.h @@ -111,7 +111,7 @@ struct TaskInput /** * @brief This indicates if a seed was provided * @details In the case of the raster process planner a skeleton seed is provided which make it - * computationaly intensive to determine if a seed was provide so this was added. + * computationally intensive to determine if a seed was provide so this was added. */ const bool has_seed{ false }; @@ -124,7 +124,7 @@ struct TaskInput /** * @brief Gets the number of instructions contained in the TaskInput - * @return 1 instruction if not a composite, otherwise size of the composite @todo Should this be -1, becuase + * @return 1 instruction if not a composite, otherwise size of the composite @todo Should this be -1, because * composite size could be 1, 0, or other? */ std::size_t size(); @@ -182,19 +182,19 @@ struct TaskInput /** @brief Results/Seed for this process */ Instruction* results_; - /** @brief The indicies used to access this process inputs instructions and results */ + /** @brief The indices used to access this process inputs instructions and results */ std::vector instruction_indice_; - /** @brief This proccess inputs start instruction */ + /** @brief This process inputs start instruction */ Instruction start_instruction_{ NullInstruction() }; - /** @brief Indices to the start instruction in the results data struction */ + /** @brief Indices to the start instruction in the results data structure */ std::vector start_instruction_indice_; - /** @brief This proccess inputs end instruction */ + /** @brief This process inputs end instruction */ Instruction end_instruction_{ NullInstruction() }; - /** @brief Indices to the end instruction in the results data struction */ + /** @brief Indices to the end instruction in the results data structure */ std::vector end_instruction_indice_; /** @brief Used to store if process input is aborted which is thread safe */ diff --git a/tesseract_process_managers/include/tesseract_process_managers/task_generators/check_input_task_generator.h b/tesseract_process_managers/include/tesseract_process_managers/task_generators/check_input_task_generator.h new file mode 100644 index 00000000000..eddb81a32f6 --- /dev/null +++ b/tesseract_process_managers/include/tesseract_process_managers/task_generators/check_input_task_generator.h @@ -0,0 +1,79 @@ +/** + * @file check_input_task_generator.h + * @brief Process generator for checking input data structure + * + * @author Levi Armstrong + * @date November 2. 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_planning +{ +using CheckInputFn = std::function; + +class CheckInputTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + CheckInputTaskGenerator(std::string name = "Check TaksInput"); + CheckInputTaskGenerator(CheckInputFn fn, std::string name = "Check TaksInput"); + + ~CheckInputTaskGenerator() override = default; + CheckInputTaskGenerator(const CheckInputTaskGenerator&) = delete; + CheckInputTaskGenerator& operator=(const CheckInputTaskGenerator&) = delete; + CheckInputTaskGenerator(CheckInputTaskGenerator&&) = delete; + CheckInputTaskGenerator& operator=(CheckInputTaskGenerator&&) = delete; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; + + void process(TaskInput input, std::size_t unique_id) const override; + +protected: + CheckInputFn fn_; + + /** + * @brief Checks that the TaskInput is in the correct format. + * @param input TaskInput to be checked + * @return True if in the correct format + */ + static bool checkTaskInput(const TaskInput& input); +}; + +class CheckInputTaskInfo : public TaskInfo +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CheckInputTaskInfo(std::size_t unique_id, std::string name = "Check TaksInput"); +}; +} // namespace tesseract_planning + +#endif // TESSERACT_PROCESS_MANAGERS_CHECK_INPUT_TASK_GENERATOR_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/task_generators/has_seed_task_generator.h b/tesseract_process_managers/include/tesseract_process_managers/task_generators/has_seed_task_generator.h new file mode 100644 index 00000000000..6b0062259f2 --- /dev/null +++ b/tesseract_process_managers/include/tesseract_process_managers/task_generators/has_seed_task_generator.h @@ -0,0 +1,61 @@ +/** + * @file has_seed_task_generator.h + * @brief Process generator for checking if the request already has a seed + * + * @author Levi Armstrong + * @date November 2. 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H +#define TESSERACT_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H + +#include + +namespace tesseract_planning +{ +class HasSeedTaskGenerator : public TaskGenerator +{ +public: + using UPtr = std::unique_ptr; + + HasSeedTaskGenerator(std::string name = "Has Seed"); + + ~HasSeedTaskGenerator() override = default; + HasSeedTaskGenerator(const HasSeedTaskGenerator&) = delete; + HasSeedTaskGenerator& operator=(const HasSeedTaskGenerator&) = delete; + HasSeedTaskGenerator(HasSeedTaskGenerator&&) = delete; + HasSeedTaskGenerator& operator=(HasSeedTaskGenerator&&) = delete; + + int conditionalProcess(TaskInput input, std::size_t unique_id) const override; + + void process(TaskInput input, std::size_t unique_id) const override; +}; + +class HasSeedTaskInfo : public TaskInfo +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + HasSeedTaskInfo(std::size_t unique_id, std::string name = "Has Seed"); +}; +} // namespace tesseract_planning + +#endif // TESSERACT_PROCESS_MANAGERS_HAS_SEED_TASK_GENERATOR_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h deleted file mode 100644 index 756c7d22cac..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/cartesian_taskflow.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file cartesian_taskflow.h - * @brief Cartesian Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_CARTESIAN_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_CARTESIAN_TASKFLOW_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -struct CartesianTaskflowParams -{ - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class CartesianTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - CartesianTaskflow(CartesianTaskflowParams params, std::string name = "CartesianTaskflow"); - ~CartesianTaskflow() override = default; - CartesianTaskflow(const CartesianTaskflow&) = delete; - CartesianTaskflow& operator=(const CartesianTaskflow&) = delete; - CartesianTaskflow(CartesianTaskflow&&) = delete; - CartesianTaskflow& operator=(CartesianTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - CartesianTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_CARTESIAN_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h deleted file mode 100644 index 52aacf5eae8..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/descartes_taskflow.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file descartes_taskflow.h - * @brief Descartes Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_DESCARTES_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_DESCARTES_TASKFLOW_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -struct DescartesTaskflowParams -{ - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class DescartesTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - DescartesTaskflow(DescartesTaskflowParams params, std::string name = "DescartesTaskflow"); - ~DescartesTaskflow() override = default; - DescartesTaskflow(const DescartesTaskflow&) = delete; - DescartesTaskflow& operator=(const DescartesTaskflow&) = delete; - DescartesTaskflow(DescartesTaskflow&&) = delete; - DescartesTaskflow& operator=(DescartesTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - DescartesTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_DESCARTES_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h deleted file mode 100644 index fea51877486..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/freespace_taskflow.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @file freespace_taskflow.h - * @brief Freespace Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_FREESPACE_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_FREESPACE_TASKFLOW_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -enum class FreespaceTaskflowType : int -{ - DEFAULT = 0, /**< @brief This will run omp followed by trajopt */ - TRAJOPT_FIRST = 1, /**< @brief This will run trajopt first then if it fails it will run ompl followed by trajopt */ -}; - -struct FreespaceTaskflowParams -{ - FreespaceTaskflowType type{ FreespaceTaskflowType::DEFAULT }; - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class FreespaceTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - FreespaceTaskflow(FreespaceTaskflowParams params, std::string name = "FreespaceTaskflow"); - ~FreespaceTaskflow() override = default; - FreespaceTaskflow(const FreespaceTaskflow&) = delete; - FreespaceTaskflow& operator=(const FreespaceTaskflow&) = delete; - FreespaceTaskflow(FreespaceTaskflow&&) = delete; - FreespaceTaskflow& operator=(FreespaceTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - FreespaceTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_FREESPACE_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h index bb138a8e9b4..54146396569 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h +++ b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/graph_taskflow.h @@ -84,7 +84,7 @@ class GraphTaskflow : public TaskflowGenerator int addNode(TaskGenerator::UPtr process, bool is_conditional = false); /** - * @brief Adds directed edges from a source node to desintation nodes in the taskflow graph + * @brief Adds directed edges from a source node to destination nodes in the taskflow graph * @details If source is a non-conditional task, it is only relevant to provide one destination as the output of a * non-conditional tf::Task is void. If source is a conditional task, the order of the destinations should correspond * to the integer output of the conditional tf::Task. For example,if the output of the conditional tf::Task is 0, the diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h deleted file mode 100644 index 31417acc7bf..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/ompl_taskflow.h +++ /dev/null @@ -1,76 +0,0 @@ -/** - * @file ompl_taskflow.h - * @brief OMPL Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_OMPL_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_OMPL_TASKFLOW_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -struct OMPLTaskflowParams -{ - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class OMPLTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - OMPLTaskflow(OMPLTaskflowParams params, std::string name = "OMPLTaskflow"); - ~OMPLTaskflow() override = default; - OMPLTaskflow(const OMPLTaskflow&) = delete; - OMPLTaskflow& operator=(const OMPLTaskflow&) = delete; - OMPLTaskflow(OMPLTaskflow&&) = delete; - OMPLTaskflow& operator=(OMPLTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - OMPLTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_OMPL_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_ifopt_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_ifopt_taskflow.h deleted file mode 100644 index c1d4bc752c4..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_ifopt_taskflow.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file trajopt_ifopt_taskflow.h - * @brief TrajOpt IFOPT Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_TRAJOPT_IFOPT_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_TRAJOPT_IFOPT_TASKFLOW_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -struct TrajOptIfoptTaskflowParams -{ - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class TrajOptIfoptTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - TrajOptIfoptTaskflow(TrajOptIfoptTaskflowParams params, std::string name = "TrajOptIfoptTaskflow"); - ~TrajOptIfoptTaskflow() override = default; - TrajOptIfoptTaskflow(const TrajOptIfoptTaskflow&) = delete; - TrajOptIfoptTaskflow& operator=(const TrajOptIfoptTaskflow&) = delete; - TrajOptIfoptTaskflow(TrajOptIfoptTaskflow&&) = delete; - TrajOptIfoptTaskflow& operator=(TrajOptIfoptTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - TrajOptIfoptTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_TRAJOPT_IFOPT_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h b/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h deleted file mode 100644 index 4c8acf5130e..00000000000 --- a/tesseract_process_managers/include/tesseract_process_managers/taskflow_generators/trajopt_taskflow.h +++ /dev/null @@ -1,77 +0,0 @@ -/** - * @file trajopt_taskflow.h - * @brief TrajOpt Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_PROCESS_MANAGERS_TRAJOPT_TASKFLOW_H -#define TESSERACT_PROCESS_MANAGERS_TRAJOPT_TASKFLOW_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -struct TrajOptTaskflowParams -{ - bool enable_post_contact_discrete_check{ true }; - bool enable_post_contact_continuous_check{ false }; - bool enable_time_parameterization{ true }; -}; - -class TrajOptTaskflow : public TaskflowGenerator -{ -public: - using UPtr = std::unique_ptr; - - TrajOptTaskflow(TrajOptTaskflowParams params, std::string name = "TrajOptTaskflow"); - ~TrajOptTaskflow() override = default; - TrajOptTaskflow(const TrajOptTaskflow&) = delete; - TrajOptTaskflow& operator=(const TrajOptTaskflow&) = delete; - TrajOptTaskflow(TrajOptTaskflow&&) = delete; - TrajOptTaskflow& operator=(TrajOptTaskflow&&) = delete; - - const std::string& getName() const override; - - TaskflowContainer generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) override; - - /** - * @brief Checks that the TaskInput is in the correct format. - * @param input TaskInput to be checked - * @return True if in the correct format - */ - static bool checkTaskInput(const TaskInput& input); - -private: - std::string name_; - TrajOptTaskflowParams params_; -}; -} // namespace tesseract_planning -#endif // TESSERACT_PROCESS_MANAGERS_TRAJOPT_TASKFLOW_H diff --git a/tesseract_process_managers/include/tesseract_process_managers/utils/taskflow_visualization_utils.h b/tesseract_process_managers/include/tesseract_process_managers/utils/taskflow_visualization_utils.h index c3bb95d1af0..c51d8d5da6f 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/utils/taskflow_visualization_utils.h +++ b/tesseract_process_managers/include/tesseract_process_managers/utils/taskflow_visualization_utils.h @@ -52,7 +52,7 @@ struct TaskDisplayInfo * @param task_info_map Key: Task name, Value: Information about that task to add to the visualization */ inline void dump(std::ostream& os, - const GraphTaskflow::UPtr& graph, + const GraphTaskflow& graph, const std::unordered_map& task_info_map = std::unordered_map()) { @@ -61,7 +61,7 @@ inline void dump(std::ostream& os, os << "subgraph cluster" << " {\nlabel=\"Taskflow:\";\n"; { - const std::vector& nodes = graph->getNodes(); + const std::vector& nodes = graph.getNodes(); for (std::size_t idx = 0; idx < nodes.size(); idx++) { const GraphTaskflowNode& node = nodes[idx]; diff --git a/tesseract_process_managers/src/core/default_process_planners.cpp b/tesseract_process_managers/src/core/default_process_planners.cpp index 80e4bb3930a..a72db0b1388 100644 --- a/tesseract_process_managers/src/core/default_process_planners.cpp +++ b/tesseract_process_managers/src/core/default_process_planners.cpp @@ -25,12 +25,6 @@ */ #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -38,55 +32,431 @@ #include #include #include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include namespace tesseract_planning { -TaskflowGenerator::UPtr createTrajOptGenerator() +TaskflowGenerator::UPtr createTrajOptGenerator(bool check_input, bool post_collisin_check) +{ + auto tf = std::make_unique("TrajOptTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup TrajOpt + auto motion_planner = std::make_shared(); + int motion_planner_task = tf->addNode(std::make_unique(motion_planner), true); + + // Setup post collision check + int contact_check_task{ std::numeric_limits::min() }; + if (post_collisin_check) + contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { motion_planner_task }); + + if (post_collisin_check) + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + else + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; +} + +TaskflowGenerator::UPtr createTrajOptIfoptGenerator(bool check_input, bool post_collisin_check) +{ + auto tf = std::make_unique("TrajOptIfoptTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup TrajOpt IFOPT + auto motion_planner = std::make_shared(); + int motion_planner_task = tf->addNode(std::make_unique(motion_planner), true); + + // Setup post collision check + int contact_check_task{ std::numeric_limits::min() }; + if (post_collisin_check) + contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { motion_planner_task }); + + if (post_collisin_check) + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + else + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; +} + +TaskflowGenerator::UPtr createOMPLGenerator(bool check_input, bool post_collisin_check) { - TrajOptTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("OMPLTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup OMPL + auto motion_planner = std::make_shared(); + int motion_planner_task = tf->addNode(std::make_unique(motion_planner), true); + + // Setup post collision check + int contact_check_task{ std::numeric_limits::min() }; + if (post_collisin_check) + contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { motion_planner_task }); + + if (post_collisin_check) + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + else + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } -TaskflowGenerator::UPtr createTrajOptIfoptGenerator() +TaskflowGenerator::UPtr createDescartesGenerator(bool check_input, bool post_collisin_check) { - TrajOptIfoptTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("DescartesTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup Descartes + auto motion_planner = std::make_shared(); + int motion_planner_task = tf->addNode(std::make_unique(motion_planner), true); + + // Setup post collision check + int contact_check_task{ std::numeric_limits::min() }; + if (post_collisin_check) + contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { motion_planner_task }); + + if (post_collisin_check) + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + else + { + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + } + + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } -TaskflowGenerator::UPtr createOMPLGenerator() +TaskflowGenerator::UPtr createDescartesOnlyGenerator(bool check_input) { - OMPLTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("DescartesTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup Descartes + auto motion_planner = std::make_shared(); + int motion_planner_task = tf->addNode(std::make_unique(motion_planner), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { motion_planner_task }); + tf->addEdges(motion_planner_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } -TaskflowGenerator::UPtr createDescartesGenerator() +TaskflowGenerator::UPtr createCartesianGenerator(bool check_input) { - DescartesTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("CartesianTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup Descartes + auto descartes_planner = std::make_shared(); + int descartes_planner_task = tf->addNode(std::make_unique(descartes_planner), true); + + // Setup TrajOpt + auto trajopt_planner = std::make_shared(); + int trajopt_planner_task = tf->addNode(std::make_unique(trajopt_planner), true); + + // Setup post collision check + int contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { descartes_planner_task }); + tf->addEdges(descartes_planner_task, { GraphTaskflow::ERROR_NODE, trajopt_planner_task }); + tf->addEdges(trajopt_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } -TaskflowGenerator::UPtr createCartesianGenerator() +TaskflowGenerator::UPtr createFreespaceGenerator(bool check_input) { - CartesianTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("FreespaceTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup OMPL + auto ompl_planner = std::make_shared(); + int ompl_planner_task = tf->addNode(std::make_unique(ompl_planner), true); + + // Setup TrajOpt + auto trajopt_planner = std::make_shared(); + int trajopt_planner_task = tf->addNode(std::make_unique(trajopt_planner), true); + + // Setup post collision check + int contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { ompl_planner_task }); + tf->addEdges(ompl_planner_task, { GraphTaskflow::ERROR_NODE, trajopt_planner_task }); + tf->addEdges(trajopt_planner_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } -TaskflowGenerator::UPtr createFreespaceGenerator() +TaskflowGenerator::UPtr createFreespaceTrajOptFirstGenerator(bool check_input) { - FreespaceTaskflowParams params; - return std::make_unique(params); + auto tf = std::make_unique("FreespaceTrajOptFirstTaskflow"); + + int check_input_task{ std::numeric_limits::min() }; + if (check_input) + check_input_task = tf->addNode(std::make_unique(), true); + + // Check if seed was provided + int has_seed_task = tf->addNode(std::make_unique(), true); + + // Simple planner as interpolator + auto interpolator = std::make_shared("Interpolator"); + int interpolator_task = tf->addNode(std::make_unique(interpolator), true); + + // Setup Seed Min Length Process Generator + // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is + // to short. + int seed_min_length_task = tf->addNode(std::make_unique()); + + // Setup TrajOpt + auto trajopt_planner1 = std::make_shared(); + int trajopt_planner1_task = tf->addNode(std::make_unique(trajopt_planner1), true); + + // Setup OMPL + auto ompl_planner = std::make_shared(); + int ompl_planner_task = tf->addNode(std::make_unique(ompl_planner), true); + + // Setup TrajOpt + auto trajopt_planner2 = std::make_shared(); + int trajopt_planner2_task = tf->addNode(std::make_unique(trajopt_planner2), true); + + // Setup post collision check + int contact_check_task = tf->addNode(std::make_unique(), true); + + // Setup time parameterization + int time_parameterization_task = tf->addNode(std::make_unique(), true); + + if (check_input) + tf->addEdges(check_input_task, { GraphTaskflow::ERROR_NODE, has_seed_task }); + + tf->addEdges(has_seed_task, { interpolator_task, seed_min_length_task }); + tf->addEdges(interpolator_task, { GraphTaskflow::ERROR_NODE, seed_min_length_task }); + tf->addEdges(seed_min_length_task, { trajopt_planner1_task }); + tf->addEdges(trajopt_planner1_task, { ompl_planner_task, contact_check_task }); + tf->addEdges(ompl_planner_task, { GraphTaskflow::ERROR_NODE, trajopt_planner2_task }); + tf->addEdges(trajopt_planner2_task, { GraphTaskflow::ERROR_NODE, contact_check_task }); + tf->addEdges(contact_check_task, { GraphTaskflow::ERROR_NODE, time_parameterization_task }); + tf->addEdges(time_parameterization_task, { GraphTaskflow::ERROR_NODE, GraphTaskflow::DONE_NODE }); + + return tf; } TaskflowGenerator::UPtr createRasterGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams fparams; - TaskflowGenerator::UPtr freespace_task = std::make_unique(fparams); - TaskflowGenerator::UPtr transition_task = std::make_unique(fparams); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cparams; - TaskflowGenerator::UPtr raster_task = std::make_unique(cparams); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -95,31 +465,20 @@ TaskflowGenerator::UPtr createRasterGenerator() TaskflowGenerator::UPtr createRasterOnlyGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams tparams; - TaskflowGenerator::UPtr transition_task = std::make_unique(tparams); + TaskflowGenerator::UPtr transition_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cparams; - TaskflowGenerator::UPtr raster_task = std::make_unique(cparams); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); return std::make_unique(std::move(transition_task), std::move(raster_task)); } TaskflowGenerator::UPtr createRasterGlobalGenerator() { - DescartesTaskflowParams global_params; - global_params.enable_post_contact_discrete_check = false; - global_params.enable_post_contact_continuous_check = false; - global_params.enable_time_parameterization = false; - TaskflowGenerator::UPtr global_task = std::make_unique(global_params); - - FreespaceTaskflowParams freespace_params; - freespace_params.type = FreespaceTaskflowType::TRAJOPT_FIRST; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(freespace_params); - - TrajOptTaskflowParams raster_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(raster_params); + TaskflowGenerator::UPtr global_task = createDescartesOnlyGenerator(false); + TaskflowGenerator::UPtr freespace_task = createFreespaceTrajOptFirstGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceTrajOptFirstGenerator(false); + TaskflowGenerator::UPtr raster_task = createTrajOptGenerator(false); return std::make_unique( std::move(global_task), std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -128,13 +487,11 @@ TaskflowGenerator::UPtr createRasterGlobalGenerator() TaskflowGenerator::UPtr createRasterDTGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams fparams; - TaskflowGenerator::UPtr freespace_task = std::make_unique(fparams); - TaskflowGenerator::UPtr transition_task = std::make_unique(fparams); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cparams; - TaskflowGenerator::UPtr raster_task = std::make_unique(cparams); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -143,13 +500,11 @@ TaskflowGenerator::UPtr createRasterDTGenerator() TaskflowGenerator::UPtr createRasterWAADGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams fparams; - TaskflowGenerator::UPtr freespace_task = std::make_unique(fparams); - TaskflowGenerator::UPtr transition_task = std::make_unique(fparams); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cparams; - TaskflowGenerator::UPtr raster_task = std::make_unique(cparams); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -158,13 +513,11 @@ TaskflowGenerator::UPtr createRasterWAADGenerator() TaskflowGenerator::UPtr createRasterWAADDTGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams fparams; - TaskflowGenerator::UPtr freespace_task = std::make_unique(fparams); - TaskflowGenerator::UPtr transition_task = std::make_unique(fparams); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cparams; - TaskflowGenerator::UPtr raster_task = std::make_unique(cparams); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -172,18 +525,9 @@ TaskflowGenerator::UPtr createRasterWAADDTGenerator() TaskflowGenerator::UPtr createRasterOnlyGlobalGenerator() { - DescartesTaskflowParams global_params; - global_params.enable_post_contact_discrete_check = false; - global_params.enable_post_contact_continuous_check = false; - global_params.enable_time_parameterization = false; - TaskflowGenerator::UPtr global_task = std::make_unique(global_params); - - FreespaceTaskflowParams transition_params; - transition_params.type = FreespaceTaskflowType::TRAJOPT_FIRST; - TaskflowGenerator::UPtr transition_task = std::make_unique(transition_params); - - TrajOptTaskflowParams raster_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(raster_params); + TaskflowGenerator::UPtr global_task = createDescartesOnlyGenerator(false); + TaskflowGenerator::UPtr transition_task = createFreespaceTrajOptFirstGenerator(false); + TaskflowGenerator::UPtr raster_task = createTrajOptGenerator(false); return std::make_unique( std::move(global_task), std::move(transition_task), std::move(raster_task)); @@ -192,13 +536,11 @@ TaskflowGenerator::UPtr createRasterOnlyGlobalGenerator() TaskflowGenerator::UPtr createRasterCTGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams freespace_params; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cartesian_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(cartesian_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(cartesian_params); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); + TaskflowGenerator::UPtr transition_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -207,9 +549,8 @@ TaskflowGenerator::UPtr createRasterCTGenerator() TaskflowGenerator::UPtr createRasterOnlyCTGenerator() { // Create Transition and Raster Taskflow - CartesianTaskflowParams cartesian_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(cartesian_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(cartesian_params); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); + TaskflowGenerator::UPtr transition_task = createCartesianGenerator(false); return std::make_unique(std::move(transition_task), std::move(raster_task)); } @@ -217,13 +558,11 @@ TaskflowGenerator::UPtr createRasterOnlyCTGenerator() TaskflowGenerator::UPtr createRasterCTDTGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams freespace_params; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cartesian_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(cartesian_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(cartesian_params); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); + TaskflowGenerator::UPtr transition_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -232,13 +571,11 @@ TaskflowGenerator::UPtr createRasterCTDTGenerator() TaskflowGenerator::UPtr createRasterCTWAADGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams freespace_params; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cartesian_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(cartesian_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(cartesian_params); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); + TaskflowGenerator::UPtr transition_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -247,13 +584,11 @@ TaskflowGenerator::UPtr createRasterCTWAADGenerator() TaskflowGenerator::UPtr createRasterCTWAADDTGenerator() { // Create Freespace and Transition Taskflows - FreespaceTaskflowParams freespace_params; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); + TaskflowGenerator::UPtr freespace_task = createFreespaceGenerator(false); // Create Raster Taskflow - CartesianTaskflowParams cartesian_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(cartesian_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(cartesian_params); + TaskflowGenerator::UPtr raster_task = createCartesianGenerator(false); + TaskflowGenerator::UPtr transition_task = createCartesianGenerator(false); return std::make_unique( std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -261,19 +596,10 @@ TaskflowGenerator::UPtr createRasterCTWAADDTGenerator() TaskflowGenerator::UPtr createRasterGlobalCTGenerator() { - DescartesTaskflowParams global_params; - global_params.enable_post_contact_discrete_check = false; - global_params.enable_post_contact_continuous_check = false; - global_params.enable_time_parameterization = false; - TaskflowGenerator::UPtr global_task = std::make_unique(global_params); - - FreespaceTaskflowParams freespace_params; - freespace_params.type = FreespaceTaskflowType::TRAJOPT_FIRST; - TaskflowGenerator::UPtr freespace_task = std::make_unique(freespace_params); - - TrajOptTaskflowParams raster_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(raster_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(raster_params); + TaskflowGenerator::UPtr global_task = createDescartesOnlyGenerator(false); + TaskflowGenerator::UPtr freespace_task = createFreespaceTrajOptFirstGenerator(false); + TaskflowGenerator::UPtr raster_task = createTrajOptGenerator(false); + TaskflowGenerator::UPtr transition_task = createTrajOptGenerator(false); return std::make_unique( std::move(global_task), std::move(freespace_task), std::move(transition_task), std::move(raster_task)); @@ -281,15 +607,9 @@ TaskflowGenerator::UPtr createRasterGlobalCTGenerator() TaskflowGenerator::UPtr createRasterOnlyGlobalCTGenerator() { - DescartesTaskflowParams global_params; - global_params.enable_post_contact_discrete_check = false; - global_params.enable_post_contact_continuous_check = false; - global_params.enable_time_parameterization = false; - TaskflowGenerator::UPtr global_task = std::make_unique(global_params); - - TrajOptTaskflowParams raster_params; - TaskflowGenerator::UPtr raster_task = std::make_unique(raster_params); - TaskflowGenerator::UPtr transition_task = std::make_unique(raster_params); + TaskflowGenerator::UPtr global_task = createDescartesOnlyGenerator(false); + TaskflowGenerator::UPtr raster_task = createTrajOptGenerator(false); + TaskflowGenerator::UPtr transition_task = createTrajOptGenerator(false); return std::make_unique( std::move(global_task), std::move(transition_task), std::move(raster_task)); diff --git a/tesseract_process_managers/src/task_generators/check_input_task_generator.cpp b/tesseract_process_managers/src/task_generators/check_input_task_generator.cpp new file mode 100644 index 00000000000..7f117a2b853 --- /dev/null +++ b/tesseract_process_managers/src/task_generators/check_input_task_generator.cpp @@ -0,0 +1,80 @@ +/** + * @file check_input_task_generator.h + * @brief Process generator for checking input data structure + * + * @author Levi Armstrong + * @date November 2. 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + +#include + +namespace tesseract_planning +{ +CheckInputTaskGenerator::CheckInputTaskGenerator(std::string name) + : TaskGenerator(std::move(name)), fn_(tesseract_planning::CheckInputTaskGenerator::checkTaskInput) +{ +} + +CheckInputTaskGenerator::CheckInputTaskGenerator(CheckInputFn fn, std::string name) + : TaskGenerator(std::move(name)), fn_(std::move(fn)) +{ +} + +int CheckInputTaskGenerator::conditionalProcess(TaskInput input, std::size_t /*unique_id*/) const +{ + return ((fn_(input)) ? 1 : 0); +} + +void CheckInputTaskGenerator::process(TaskInput input, std::size_t unique_id) const +{ + conditionalProcess(input, unique_id); +} + +bool CheckInputTaskGenerator::checkTaskInput(const TaskInput& input) +{ + // Check Input + if (!input.env) + { + CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); + return false; + } + + // Check the overall input + const Instruction* input_instruction = input.getInstruction(); + if (!isCompositeInstruction(*input_instruction)) + { + CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); + return false; + } + + return true; +} + +CheckInputTaskInfo::CheckInputTaskInfo(std::size_t unique_id, std::string name) : TaskInfo(unique_id, std::move(name)) +{ +} +} // namespace tesseract_planning diff --git a/tesseract_process_managers/src/task_generators/has_seed_task_generator.cpp b/tesseract_process_managers/src/task_generators/has_seed_task_generator.cpp new file mode 100644 index 00000000000..264cc300e50 --- /dev/null +++ b/tesseract_process_managers/src/task_generators/has_seed_task_generator.cpp @@ -0,0 +1,51 @@ +/** + * @file has_seed_task_generator.cpp + * @brief Process generator for checking if seed exists + * + * @author Levi Armstrong + * @date November 2. 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + +#include +#include + +namespace tesseract_planning +{ +HasSeedTaskGenerator::HasSeedTaskGenerator(std::string name) : TaskGenerator(std::move(name)) {} + +int HasSeedTaskGenerator::conditionalProcess(TaskInput input, std::size_t /*unique_id*/) const +{ + return hasSeedTask(input); +} + +void HasSeedTaskGenerator::process(TaskInput input, std::size_t unique_id) const +{ + conditionalProcess(input, unique_id); +} + +HasSeedTaskInfo::HasSeedTaskInfo(std::size_t unique_id, std::string name) : TaskInfo(unique_id, std::move(name)) {} +} // namespace tesseract_planning diff --git a/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp deleted file mode 100644 index 4764f3f184e..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/cartesian_taskflow.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/** - * @file cartesian_taskflow.cpp - * @brief Cartesian Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -CartesianTaskflow::CartesianTaskflow(CartesianTaskflowParams params, std::string name) - : name_(std::move(name)), params_(params) -{ -} - -const std::string& CartesianTaskflow::getName() const { return name_; } - -TaskflowContainer CartesianTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - TaskflowContainer container; - container.taskflow = std::make_unique(name_); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task descartes_task = container.taskflow->placeholder(); - tf::Task seed_min_length_task = container.taskflow->placeholder(); - tf::Task trajopt_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, seed_min_length_task); - interpolator_task.precede(error_task, seed_min_length_task); - seed_min_length_task.precede(descartes_task); - descartes_task.precede(error_task, trajopt_task); - - // Setup Interpolator - auto interpolator = std::make_shared("Interpolator"); - TaskGenerator::UPtr interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - // Setup Seed Min Length Process Generator - // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is - // to short. - TaskGenerator::UPtr seed_min_length_generator = std::make_unique(); - seed_min_length_generator->assignTask(input, seed_min_length_task); - container.generators.push_back(std::move(seed_min_length_generator)); - - // Setup Descartes - auto descartes_planner = std::make_shared>(); - auto descartes_generator = std::make_unique(descartes_planner); - descartes_generator->assignConditionalTask(input, descartes_task); - container.generators.push_back(std::move(descartes_generator)); - - // Setup TrajOpt - auto trajopt_planner = std::make_shared(); - TaskGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); - trajopt_generator->assignConditionalTask(input, trajopt_task); - container.generators.push_back(std::move(trajopt_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - trajopt_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - trajopt_task.precede(error_task, done_task); - } - - return container; -} - -bool CartesianTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp deleted file mode 100644 index 84f0cb7468e..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/descartes_taskflow.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/** - * @file descartes_taskflow.cpp - * @brief Descartes Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -DescartesTaskflow::DescartesTaskflow(DescartesTaskflowParams params, std::string name) - : name_(std::move(name)), params_(params) -{ -} - -const std::string& DescartesTaskflow::getName() const { return name_; } - -TaskflowContainer DescartesTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - TaskflowContainer container; - container.taskflow = std::make_unique(name_); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task descartes_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, descartes_task); - interpolator_task.precede(error_task, descartes_task); - - // Setup Interpolator - auto interpolator = std::make_shared("Interpolator"); - auto interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - // Setup Descartes - auto descartes_planner = std::make_shared>(); - auto descartes_generator = std::make_unique(descartes_planner); - descartes_generator->assignConditionalTask(input, descartes_task); - container.generators.push_back(std::move(descartes_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - descartes_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - descartes_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - descartes_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - descartes_task.precede(error_task, done_task); - } - - return container; -} - -bool DescartesTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp deleted file mode 100644 index bc54f7155c9..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/freespace_taskflow.cpp +++ /dev/null @@ -1,251 +0,0 @@ -/** - * @file freespace_taskflow.cpp - * @brief Freespace Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -FreespaceTaskflow::FreespaceTaskflow(FreespaceTaskflowParams params, std::string name) - : name_(std::move(name)), params_(params) -{ -} - -const std::string& FreespaceTaskflow::getName() const { return name_; } - -TaskflowContainer FreespaceTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - std::string name = name_; - if (params_.type == FreespaceTaskflowType::TRAJOPT_FIRST) - name += "(TrajOpt First)"; - else - name += "(Default)"; - - TaskflowContainer container; - container.taskflow = std::make_unique(name); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task ompl_task = container.taskflow->placeholder(); - tf::Task seed_min_length_task = container.taskflow->placeholder(); - tf::Task trajopt_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, seed_min_length_task); - interpolator_task.precede(error_task, seed_min_length_task); - - // Define Tasks - auto interpolator = std::make_shared("Interpolator"); - auto interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - auto seed_min_length_generator = std::make_unique(); - seed_min_length_generator->assignTask(input, seed_min_length_task); - container.generators.push_back(std::move(seed_min_length_generator)); - - auto ompl_planner = std::make_shared(); - auto ompl_generator = std::make_unique(ompl_planner); - ompl_generator->assignTask(input, ompl_task); - container.generators.push_back(std::move(ompl_generator)); - - auto trajopt_planner = std::make_shared(); - auto trajopt_generator = std::make_unique(trajopt_planner); - trajopt_generator->assignConditionalTask(input, trajopt_task); - container.generators.push_back(std::move(trajopt_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - if (params_.type == FreespaceTaskflowType::TRAJOPT_FIRST) - { - seed_min_length_task.precede(trajopt_task); - - tf::Task trajopt_second_task = container.taskflow->placeholder(); - - // Setup TrajOpt - auto trajopt_planner2 = std::make_shared(); - TaskGenerator::UPtr trajopt_generator2 = std::make_unique(trajopt_planner2); - trajopt_generator2->assignConditionalTask(input, trajopt_second_task); - container.generators.push_back(std::move(trajopt_generator2)); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(ompl_task, contact_task); - ompl_task.precede(trajopt_second_task); - trajopt_second_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(ompl_task, contact_task); - ompl_task.precede(trajopt_second_task); - trajopt_second_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - trajopt_task.precede(ompl_task, time_task); - ompl_task.precede(trajopt_second_task); - trajopt_second_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - trajopt_task.precede(ompl_task, done_task); - ompl_task.precede(trajopt_second_task); - trajopt_second_task.precede(error_task, done_task); - } - } - else - { - seed_min_length_task.precede(ompl_task); - ompl_task.precede(trajopt_task); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - trajopt_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - trajopt_task.precede(error_task, done_task); - } - } - - return container; -} - -bool FreespaceTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp deleted file mode 100644 index 2deed7d70a7..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/ompl_taskflow.cpp +++ /dev/null @@ -1,166 +0,0 @@ -/** - * @file ompl_taskflow.cpp - * @brief OMPL Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -OMPLTaskflow::OMPLTaskflow(OMPLTaskflowParams params, std::string name) : name_(std::move(name)), params_(params) {} - -const std::string& OMPLTaskflow::getName() const { return name_; } - -TaskflowContainer OMPLTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - TaskflowContainer container; - container.taskflow = std::make_unique(name_); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task ompl_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, ompl_task); - interpolator_task.precede(error_task, ompl_task); - - // Setup Interpolator - auto interpolator = std::make_shared("Interpolator"); - auto interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - // Setup TrajOpt - auto ompl_planner = std::make_shared(); - auto ompl_generator = std::make_unique(ompl_planner); - ompl_generator->assignConditionalTask(input, ompl_task); - container.generators.push_back(std::move(ompl_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - ompl_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - ompl_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - ompl_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - ompl_task.precede(error_task, done_task); - } - - return container; -} - -bool OMPLTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/src/taskflow_generators/trajopt_ifopt_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/trajopt_ifopt_taskflow.cpp deleted file mode 100644 index 086944cf0ee..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/trajopt_ifopt_taskflow.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/** - * @file trajopt_ifopt_taskflow.cpp - * @brief TrajOpt IFOPT Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -TrajOptIfoptTaskflow::TrajOptIfoptTaskflow(TrajOptIfoptTaskflowParams params, std::string name) - : name_(std::move(name)), params_(params) -{ -} - -const std::string& TrajOptIfoptTaskflow::getName() const { return name_; } - -TaskflowContainer TrajOptIfoptTaskflow::generateTaskflow(TaskInput input, - TaskflowVoidFn done_cb, - TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - TaskflowContainer container; - container.taskflow = std::make_unique(name_); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task seed_min_length_task = container.taskflow->placeholder(); - tf::Task trajopt_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, seed_min_length_task); - interpolator_task.precede(error_task, seed_min_length_task); - seed_min_length_task.precede(trajopt_task); - - // Setup Interpolator - auto interpolator = std::make_shared("Interpolator"); - TaskGenerator::UPtr interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - // Setup Seed Min Length Process Generator - // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is - // to short. - TaskGenerator::UPtr seed_min_length_generator = std::make_unique(); - seed_min_length_generator->assignTask(input, seed_min_length_task); - container.generators.push_back(std::move(seed_min_length_generator)); - - // Setup TrajOpt - auto trajopt_planner = std::make_shared(); - TaskGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); - trajopt_generator->assignConditionalTask(input, trajopt_task); - container.generators.push_back(std::move(trajopt_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - trajopt_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - trajopt_task.precede(error_task, done_task); - } - - return container; -} - -bool TrajOptIfoptTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp b/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp deleted file mode 100644 index 44e75aceefc..00000000000 --- a/tesseract_process_managers/src/taskflow_generators/trajopt_taskflow.cpp +++ /dev/null @@ -1,179 +0,0 @@ -/** - * @file trajopt_taskflow.cpp - * @brief TrajOpt Graph Taskflow - * - * @author Levi Armstrong - * @date August 27, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -using namespace tesseract_planning; - -TrajOptTaskflow::TrajOptTaskflow(TrajOptTaskflowParams params, std::string name) - : name_(std::move(name)), params_(params) -{ -} - -const std::string& TrajOptTaskflow::getName() const { return name_; } - -TaskflowContainer TrajOptTaskflow::generateTaskflow(TaskInput input, TaskflowVoidFn done_cb, TaskflowVoidFn error_cb) -{ - // This should make all of the isComposite checks so that you can safely cast below - if (!checkTaskInput(input)) - { - CONSOLE_BRIDGE_logError("Invalid Process Input"); - throw std::runtime_error("Invalid Process Input"); - } - - TaskflowContainer container; - container.taskflow = std::make_unique(name_); - - // Add "Error" task - auto error_fn = [=]() { failureTask(input, name_, "", error_cb); }; - tf::Task error_task = container.taskflow->emplace(error_fn).name("Error Callback"); - container.outputs.push_back(error_task); - - // Add "Done" task - auto done_fn = [=]() { successTask(input, name_, "", done_cb); }; - tf::Task done_task = container.taskflow->emplace(done_fn).name("Done Callback"); - container.outputs.push_back(done_task); - - // Add has seed check - tf::Task has_seed_task = container.taskflow->emplace([=]() { return hasSeedTask(input); }).name("Has Seed Check"); - - tf::Task interpolator_task = container.taskflow->placeholder(); - tf::Task seed_min_length_task = container.taskflow->placeholder(); - tf::Task trajopt_task = container.taskflow->placeholder(); - - has_seed_task.precede(interpolator_task, seed_min_length_task); - interpolator_task.precede(error_task, seed_min_length_task); - seed_min_length_task.precede(trajopt_task); - - // Setup Interpolator - auto interpolator = std::make_shared("Interpolator"); - TaskGenerator::UPtr interpolator_generator = std::make_unique(interpolator); - interpolator_generator->assignConditionalTask(input, interpolator_task); - container.generators.push_back(std::move(interpolator_generator)); - - // Setup Seed Min Length Process Generator - // This is required because trajopt requires a minimum length trajectory. This is used to correct the seed if it is - // to short. - TaskGenerator::UPtr seed_min_length_generator = std::make_unique(); - seed_min_length_generator->assignTask(input, seed_min_length_task); - container.generators.push_back(std::move(seed_min_length_generator)); - - // Setup TrajOpt - auto trajopt_planner = std::make_shared(); - TaskGenerator::UPtr trajopt_generator = std::make_unique(trajopt_planner); - trajopt_generator->assignConditionalTask(input, trajopt_task); - container.generators.push_back(std::move(trajopt_generator)); - - TaskGenerator::UPtr contact_check_generator; - bool has_contact_check = (params_.enable_post_contact_continuous_check || params_.enable_post_contact_discrete_check); - if (has_contact_check) - { - if (params_.enable_post_contact_continuous_check) - contact_check_generator = std::make_unique(); - else if (params_.enable_post_contact_discrete_check) - contact_check_generator = std::make_unique(); - } - - TaskGenerator::UPtr time_parameterization_generator; - if (params_.enable_time_parameterization) - time_parameterization_generator = std::make_unique(); - - // Add Final Continuous Contact Check of trajectory and Time parameterization trajectory - if (has_contact_check && params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - container.generators.push_back(std::move(contact_check_generator)); - - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - contact_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else if (has_contact_check && !params_.enable_time_parameterization) - { - tf::Task contact_task = container.taskflow->placeholder(); - contact_check_generator->assignConditionalTask(input, contact_task); - trajopt_task.precede(error_task, contact_task); - contact_task.precede(error_task, done_task); - container.generators.push_back(std::move(contact_check_generator)); - } - else if (!has_contact_check && params_.enable_time_parameterization) - { - tf::Task time_task = container.taskflow->placeholder(); - time_parameterization_generator->assignConditionalTask(input, time_task); - container.generators.push_back(std::move(time_parameterization_generator)); - trajopt_task.precede(error_task, time_task); - time_task.precede(error_task, done_task); - container.generators.push_back(std::move(time_parameterization_generator)); - } - else - { - trajopt_task.precede(error_task, done_task); - } - - return container; -} - -bool TrajOptTaskflow::checkTaskInput(const tesseract_planning::TaskInput& input) -{ - // Check Input - if (!input.env) - { - CONSOLE_BRIDGE_logError("TaskInput env is a nullptr"); - return false; - } - - // Check the overall input - const Instruction* input_instruction = input.getInstruction(); - if (!isCompositeInstruction(*input_instruction)) - { - CONSOLE_BRIDGE_logError("TaskInput Invalid: input.instructions should be a composite"); - return false; - } - - return true; -} diff --git a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp index 7ec7d01aa0d..1aae134fae7 100644 --- a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp +++ b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp @@ -21,10 +21,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include -#include -#include #include #include "raster_example_program.h"