From 0edd73c2de6d0a7af2ef5d74a2bf86fd3ee4675a Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 14:14:34 -0600 Subject: [PATCH 01/46] Added new interfaces for planner profiles; added maps for waypoint, composite, and planner profiles in profile dictionary --- .../profile_dictionary.h | 97 ++++++++++++++++++- 1 file changed, 96 insertions(+), 1 deletion(-) diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index 4973bd039fc..75d459379a1 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -36,19 +36,106 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include + #ifdef SWIG %shared_ptr(tesseract_planning::ProfileDictionary) #endif // SWIG namespace tesseract_planning { +/** + * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint. + * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler + */ +class WaypointProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + WaypointProfile() = default; + WaypointProfile(const WaypointProfile&) = delete; + WaypointProfile& operator=(const WaypointProfile&) = delete; + WaypointProfile(WaypointProfile&&) = delete; + WaypointProfile&& operator=(WaypointProfile&&) = delete; + + virtual ~WaypointProfile() = default; + + virtual std::any create(const Instruction& instruction, tesseract_environment::Environment::ConstPtr env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce a planner-specific planning profile to apply to a collection of waypoints defined in a + * composite instruction. + * @details Examples of composite profiles include costs/constraints that apply collectively to a group of waypoints + */ +class CompositeProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CompositeProfile() = default; + CompositeProfile(const CompositeProfile&) = delete; + CompositeProfile& operator=(const CompositeProfile&) = delete; + CompositeProfile(CompositeProfile&&) = delete; + CompositeProfile&& operator=(CompositeProfile&&) = delete; + + virtual ~CompositeProfile() = default; + virtual std::any create(const CompositeInstruction& instruction, + tesseract_environment::Environment::ConstPtr env) const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + +/** + * @brief Struct to produce configuration parameters for the motion planner + */ +struct PlannerProfile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PlannerProfile() = default; + PlannerProfile(const PlannerProfile&) = delete; + PlannerProfile& operator=(const PlannerProfile&) = delete; + PlannerProfile(PlannerProfile&&) = delete; + PlannerProfile&& operator=(PlannerProfile&&) = delete; + + virtual ~PlannerProfile() = default; + + virtual std::any create() const = 0; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) // NOLINT + { + } +}; + /** * @brief This class is used to store profiles for motion planning and process planning * @details This is a thread safe class * A ProfileEntry is a std::unordered_map> * - The key is the profile name * - Where std::shared_ptr is the profile - * The ProfleEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) + * The ProfileEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) * @note When adding a profile entry the T should be the base class type. */ class ProfileDictionary @@ -210,10 +297,18 @@ class ProfileDictionary .erase(profile_name); } + std::unordered_map> waypoint_profiles; + std::unordered_map> composite_profiles; + std::unordered_map> planner_profiles; + protected: std::unordered_map> profiles_; mutable std::shared_mutex mutex_; }; } // namespace tesseract_planning +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::WaypointProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::CompositeProfile); +BOOST_SERIALIZATION_ASSUME_ABSTRACT(tesseract_planning::PlannerProfile); + #endif // TESSERACT_MOTION_PLANNERS_PROFILE_DICTIONARY_H From 6cced559d7cbcfd70924cdaf8f9943ada06024c1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 14:44:14 -0600 Subject: [PATCH 02/46] Forward declare profile function argument classes --- .../profile_dictionary.h | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index 75d459379a1..01f0fab6d5e 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include #include #include @@ -36,15 +37,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include - #ifdef SWIG %shared_ptr(tesseract_planning::ProfileDictionary) #endif // SWIG +namespace tesseract_environment +{ +class Environment; +} + namespace tesseract_planning { +class Instruction; +class CompositeInstruction; + /** * @brief Struct to produce a planner-specific planning profile to apply to a single waypoint. * @details Examples of waypoint profiles might include costs/constraints for a waypoint or a waypoint sampler @@ -63,7 +69,7 @@ class WaypointProfile virtual ~WaypointProfile() = default; - virtual std::any create(const Instruction& instruction, tesseract_environment::Environment::ConstPtr env) const = 0; + virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const = 0; private: friend class boost::serialization::access; @@ -92,7 +98,7 @@ class CompositeProfile virtual ~CompositeProfile() = default; virtual std::any create(const CompositeInstruction& instruction, - tesseract_environment::Environment::ConstPtr env) const = 0; + const tesseract_environment::Environment& env) const = 0; private: friend class boost::serialization::access; From 467d5eae1e0e92e67dda041e7d98fe5f75f31c58 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 15:14:46 -0600 Subject: [PATCH 03/46] Include composite instruction and environment headers where profile dictionary is used --- tesseract_motion_planners/test/profile_dictionary_tests.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 5159c79055e..1ee93399384 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include struct ProfileBase From 6da6684e6279e1e0729e515bae4993efa4695a1b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 16:31:58 -0600 Subject: [PATCH 04/46] Revised flatten utility to ignore subsequent start state instructions in nested composites --- tesseract_command_language/src/utils/flatten_utils.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tesseract_command_language/src/utils/flatten_utils.cpp b/tesseract_command_language/src/utils/flatten_utils.cpp index 80e91c796e7..35a81652ab8 100644 --- a/tesseract_command_language/src/utils/flatten_utils.cpp +++ b/tesseract_command_language/src/utils/flatten_utils.cpp @@ -49,7 +49,7 @@ void flattenHelper(std::vector>& flattened, const flattenFilterFn& filter, bool first_composite) { - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -92,7 +92,7 @@ void flattenHelper(std::vector>& flatt const flattenFilterFn& filter, bool first_composite) { - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -143,7 +143,7 @@ void flattenToPatternHelper(std::vector>& fl return; } - if (composite.hasStartInstruction()) + if (composite.hasStartInstruction() && first_composite) if (!filter || filter(composite.getStartInstruction(), composite, first_composite)) flattened.emplace_back(composite.getStartInstruction()); @@ -196,7 +196,7 @@ void flattenToPatternHelper(std::vector Date: Fri, 7 Jan 2022 16:33:03 -0600 Subject: [PATCH 05/46] Updated simple planner interpolate composite function to fill out nested composite start instructions --- .../src/simple/profile/simple_planner_utils.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp b/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp index 77fce613e3d..d4b0b951961 100644 --- a/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp +++ b/tesseract_motion_planners/core/src/simple/profile/simple_planner_utils.cpp @@ -163,15 +163,20 @@ CompositeInstruction getInterpolatedComposite(const std::vector& jo // Get move type base on base instruction type MoveInstructionType move_type = getMoveInstructionType(base_instruction); + auto create_move_inst = [&](const Eigen::Index idx) -> MoveInstruction { + MoveInstruction inst(StateWaypoint(joint_names, states.col(idx)), move_type); + inst.setManipulatorInfo(base_instruction.getManipulatorInfo()); + inst.setDescription(base_instruction.getDescription()); + inst.setProfile(base_instruction.getProfile()); + inst.profile_overrides = base_instruction.profile_overrides; + return inst; + }; + // Convert to MoveInstructions + composite.setStartInstruction(create_move_inst(0)); for (long i = 1; i < states.cols(); ++i) { - MoveInstruction move_instruction(StateWaypoint(joint_names, states.col(i)), move_type); - move_instruction.setManipulatorInfo(base_instruction.getManipulatorInfo()); - move_instruction.setDescription(base_instruction.getDescription()); - move_instruction.setProfile(base_instruction.getProfile()); - move_instruction.profile_overrides = base_instruction.profile_overrides; - composite.push_back(move_instruction); + composite.push_back(create_move_inst(i)); } return composite; From af763b16898fdcd8591d018a9a143849ac2197e6 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 18:26:10 -0600 Subject: [PATCH 06/46] Updated instruction getter utilities --- .../utils/get_instruction_utils.h | 25 +++++++++++-------- .../src/utils/get_instruction_utils.cpp | 6 ++--- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h b/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h index 505add5f9c7..27347e31b9b 100644 --- a/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h +++ b/tesseract_command_language/include/tesseract_command_language/utils/get_instruction_utils.h @@ -35,6 +35,7 @@ namespace tesseract_planning { /** * @brief Get the first Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -46,6 +47,7 @@ const Instruction* getFirstInstruction(const CompositeInstruction& composite_ins /** * @brief Get the first Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -57,6 +59,7 @@ Instruction* getFirstInstruction(CompositeInstruction& composite_instruction, /** * @brief Get the last Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -68,6 +71,7 @@ const Instruction* getLastInstruction(const CompositeInstruction& composite_inst /** * @brief Get the last Instruction in a Composite Instruction that is identified by the filter + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -79,7 +83,7 @@ Instruction* getLastInstruction(CompositeInstruction& composite_instruction, /** * @brief Get the first Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Move Instruction (Non-Const) */ @@ -94,7 +98,7 @@ inline MoveInstruction* getFirstMoveInstruction(CompositeInstruction& composite_ /** * @brief Get the first Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Move Instruction (Const) */ @@ -109,7 +113,7 @@ inline const MoveInstruction* getFirstMoveInstruction(const CompositeInstruction /** * @brief Get the first Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Plan Instruction (Non-Const) */ @@ -124,7 +128,7 @@ inline PlanInstruction* getFirstPlanInstruction(CompositeInstruction& composite_ /** * @brief Get the first Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The first Plan Instruction (Const) */ @@ -139,7 +143,7 @@ inline const PlanInstruction* getFirstPlanInstruction(const CompositeInstruction /** * @brief Get the last Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Move Instruction (Non-Const) */ @@ -154,7 +158,7 @@ inline MoveInstruction* getLastMoveInstruction(CompositeInstruction& composite_i /** * @brief Get the last Move Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Move Instruction (Const) */ @@ -169,7 +173,7 @@ inline const MoveInstruction* getLastMoveInstruction(const CompositeInstruction& /** * @brief Get the last Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Plan Instruction (Non-Const) */ @@ -184,7 +188,7 @@ inline PlanInstruction* getLastPlanInstruction(CompositeInstruction& composite_i /** * @brief Get the last Plan Instruction in a Composite Instruction - * This does not consider the start instruction in child composite instruction + * @detials This does not consider the start instruction of nested composite instructions * @param composite_instruction Composite Instruction to search * @return The last Plan Instruction (Const) */ @@ -199,6 +203,7 @@ inline const PlanInstruction* getLastPlanInstruction(const CompositeInstruction& /** * @brief Get number of Instruction in a Composite Instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @param locate_filter The filter to indicate if an instruction should be considered * @param process_child_composites Indicate if child Composite Instructions should be searched @@ -210,7 +215,7 @@ long getInstructionCount(const CompositeInstruction& composite_instruction, /** * @brief Get number of Move Instruction in a Composite Instruction - * This does not consider the start instruction in the child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @return The number of Move Instructions */ @@ -221,7 +226,7 @@ inline long getMoveInstructionCount(const CompositeInstruction& composite_instru /** * @brief Get number of Plan Instruction in a Composite Instruction - * This does not consider the start instruction in the child composite instruction + * @details This does not consider the start instruction of nested composite instructions * @param composite_instruction The Composite Instruction to process * @return The number of Plan Instructions */ diff --git a/tesseract_command_language/src/utils/get_instruction_utils.cpp b/tesseract_command_language/src/utils/get_instruction_utils.cpp index db4957e4f1b..66fa33459d7 100644 --- a/tesseract_command_language/src/utils/get_instruction_utils.cpp +++ b/tesseract_command_language/src/utils/get_instruction_utils.cpp @@ -42,7 +42,7 @@ const Instruction* getFirstInstructionHelper(const CompositeInstruction& composi bool process_child_composites, bool first_composite) { - if (composite_instruction.hasStartInstruction()) + if (composite_instruction.hasStartInstruction() && first_composite) if (!locate_filter || locate_filter(composite_instruction.getStartInstruction(), composite_instruction, first_composite)) return &(composite_instruction.getStartInstruction()); @@ -78,7 +78,7 @@ Instruction* getFirstInstructionHelper(CompositeInstruction& composite_instructi bool process_child_composites, bool first_composite) { - if (composite_instruction.hasStartInstruction()) + if (composite_instruction.hasStartInstruction() && first_composite) if (!locate_filter || locate_filter(composite_instruction.getStartInstruction(), composite_instruction, first_composite)) return &(composite_instruction.getStartInstruction()); @@ -197,7 +197,7 @@ long getInstructionCountHelper(const CompositeInstruction& composite_instruction bool first_composite) { long cnt = 0; - if (composite_instruction.hasStartInstruction()) + if (composite_instruction.hasStartInstruction() && first_composite) if (!locate_filter || locate_filter(composite_instruction.getStartInstruction(), composite_instruction, first_composite)) ++cnt; From 8d653f464ba3adeac41f103e8f1a731ee3a56724 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 18:26:41 -0600 Subject: [PATCH 07/46] Added test for instruction getter utilities --- .../test/utils_test.cpp | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/tesseract_command_language/test/utils_test.cpp b/tesseract_command_language/test/utils_test.cpp index f47dde66d7c..d5a4572fd34 100644 --- a/tesseract_command_language/test/utils_test.cpp +++ b/tesseract_command_language/test/utils_test.cpp @@ -503,6 +503,67 @@ TEST(TesseractCommandLanguageUtilsUnit, toDelimitedFile) // NOLINT EXPECT_EQ(check, buffer.str()); } +TEST(TesseractCommandLanguageUtilsUnit, getUtils) // NOLINT +{ + const Eigen::Index dof = 6; + const std::vector names(static_cast(dof), "joint"); + + CompositeInstruction composite; + for (Eigen::Index i = 0; i < 3; ++i) + { + CompositeInstruction child; + for (Eigen::Index j = 0; j < 10; ++j) + { + if (j == 0) + { + child.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + } + else + { + child.push_back(MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::LINEAR)); + } + } + composite.push_back(child); + } + + // Add an empty composite with a start instruction to the end of the top-level composite + { + CompositeInstruction child; + child.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + composite.push_back(child); + } + + // The first move instruction should be the first instruction of the first nested composite (the top level composite + // instruction does not have a start instruction yet) + ASSERT_EQ(getFirstInstruction(composite, &moveFilter), &composite.front().as().front()); + + // Add a move instruction to the top-level composite + composite.setStartInstruction( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::START)); + // The first move instruction should now be the start instruction of the top-level composite + ASSERT_EQ(getFirstInstruction(composite, &moveFilter), &composite.getStartInstruction()); + + // The last move instruction should be the last instruction of the second to last composite instruction since the last + // nested composite is empty except for a start instruction + ASSERT_EQ(getLastInstruction(composite, &moveFilter), &composite.at(2).as().back()); + + // Add an instruction to the last nested composite instruction + composite.back().as().push_back( + MoveInstruction(JointWaypoint(names, Eigen::VectorXd::Zero(dof)), MoveInstructionType::LINEAR)); + // The last move instruction should now be the last instruction of the last nested composite + ASSERT_EQ(getLastInstruction(composite, &moveFilter), &composite.back().as().back()); + + // The total count of move instructions, excluding start instructions of nested composite instructions, should be: + // - the number of original nested composites times the number of waypoints per composite (minus one for the one + // used in the start + // instruction) + // - plus one for an initial start instruction in the top-level composite + // - plus one instruction added in the initially empty last nested composite + ASSERT_EQ(getInstructionCount(composite, &moveFilter), 3 * (10 - 1) + 2); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 6ae1bd18964b1233587933fea52f1d00ff853d81 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 10:50:07 -0600 Subject: [PATCH 08/46] Added vector insert method to composite instruction --- .../tesseract_command_language/composite_instruction.h | 1 + tesseract_command_language/src/composite_instruction.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h index 81c8f21418a..9815d79d70d 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -201,6 +201,7 @@ class CompositeInstruction iterator insert(const_iterator p, const value_type& x); iterator insert(const_iterator p, value_type&& x); iterator insert(const_iterator p, std::initializer_list l); + iterator insert(const_iterator p, std::size_t n, const value_type& x); template void insert(const_iterator pos, InputIt first, InputIt last) { diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 0dc9b05233a..58de78011a1 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -166,6 +166,10 @@ CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, st { return container_.insert(p, l); } +CompositeInstruction::iterator CompositeInstruction::insert(const_iterator p, std::size_t n, const value_type& x) +{ + return container_.insert(p, n, x); +} template CompositeInstruction::iterator CompositeInstruction::emplace(const_iterator pos, Args&&... args) From bd8871db19d303214ea4d53f18f139e526f543ef Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 10:50:33 -0600 Subject: [PATCH 09/46] Made joint waypoint isApprox method const --- .../include/tesseract_command_language/joint_waypoint.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h index 9f8e3b25a81..704fe2341c8 100644 --- a/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h +++ b/tesseract_command_language/include/tesseract_command_language/joint_waypoint.h @@ -89,7 +89,10 @@ class JointWaypoint /** @returns norm of vector */ inline double norm() const { return waypoint.norm(); } /** @returns true if two are approximate */ - inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) { return waypoint.isApprox(other, prec); } + inline bool isApprox(const Eigen::VectorXd& other, double prec = 1e-12) const + { + return waypoint.isApprox(other, prec); + } /** @returns the transpose of the joint positions */ inline ConstTransposeReturnType transpose() const { return waypoint.transpose(); } // NOLINT From 721995da52cd6a08fe0dfc611f959e2b2ddee72c Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 10:56:35 -0600 Subject: [PATCH 10/46] Removed OMPL serialization --- .../ompl/deserialize.h | 48 ------- .../ompl/serialize.h | 44 ------ .../ompl/src/deserialize.cpp | 126 ------------------ .../ompl/src/serialize.cpp | 81 ----------- 4 files changed, 299 deletions(-) delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h delete mode 100644 tesseract_motion_planners/ompl/src/deserialize.cpp delete mode 100644 tesseract_motion_planners/ompl/src/serialize.cpp diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h deleted file mode 100644 index cf7540c3468..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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_MOTION_PLANNERS_OMPL_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element); - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path); - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_DESERIALIZE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h deleted file mode 100644 index 5d087bbc17a..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file serialize.h - * @brief Provide methods for serializing ompl plans to xml - * - * @author Tyler Marr - * @date August 24, 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_MOTION_PLANNERS_OMPL_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const OMPLPlanProfile& plan_profile); - -bool toXMLFile(const OMPLPlanProfile& plan_profile, const std::string& file_path); - -std::string toXMLString(const OMPLPlanProfile& plan_profile); - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_SERIALIZE_H diff --git a/tesseract_motion_planners/ompl/src/deserialize.cpp b/tesseract_motion_planners/ompl/src/deserialize.cpp deleted file mode 100644 index fde78fad744..00000000000 --- a/tesseract_motion_planners/ompl/src/deserialize.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 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 -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile omplPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* ompl_plan_element = xml_element.FirstChildElement("OMPLPlanProfile"); - return OMPLDefaultPlanProfile(*ompl_plan_element); -} - -OMPLDefaultPlanProfile omplPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - tinyxml2::XMLError status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return omplPlanParser(*planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return omplPlanFromXMLElement(planner_xml); -} - -OMPLDefaultPlanProfile omplPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - tinyxml2::XMLError status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return omplPlanFromXMLDocument(xml_doc); -} - -OMPLDefaultPlanProfile omplPlanFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return omplPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/serialize.cpp b/tesseract_motion_planners/ompl/src/serialize.cpp deleted file mode 100644 index b2549465fe1..00000000000 --- a/tesseract_motion_planners/ompl/src/serialize.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 20, 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 -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -static const std::array VERSION{ { 1, 0, 0 } }; -static const Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const OMPLPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "OMPLDefaultProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_plan_profile = plan_profile.toXML(*doc); - xml_root->InsertEndChild(xml_plan_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const OMPLPlanProfile& plan_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Plan Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const OMPLPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return std::string(printer.CStr()); -} - -} // namespace tesseract_planning From b1a5f97d63d466b20271c48b459892751d3ee698 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:02:57 -0600 Subject: [PATCH 11/46] Deleted OMPL problem and old problem generators/profiles --- tesseract_motion_planners/ompl/CMakeLists.txt | 16 +- .../ompl/ompl_problem.h | 154 ---- .../ompl/profile/ompl_default_plan_profile.h | 161 ---- .../ompl/profile/ompl_profile.h | 104 --- .../tesseract_motion_planners/ompl/types.h | 43 - .../tesseract_motion_planners/ompl/utils.h | 32 +- .../ompl/src/ompl_problem.cpp | 38 - .../src/profile/ompl_default_plan_profile.cpp | 773 ------------------ tesseract_motion_planners/ompl/src/utils.cpp | 27 - 9 files changed, 9 insertions(+), 1339 deletions(-) delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h delete mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h delete mode 100644 tesseract_motion_planners/ompl/src/ompl_problem.cpp delete mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index 4dd3dbd52f8..11edc1406f9 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -4,19 +4,19 @@ link_directories(BEFORE ${OMPL_LIBRARY_DIRS}) # OMPL Freespace Planning Interface set(OMPL_SRC - src/ompl_motion_planner.cpp + # OMPL Interfaces src/continuous_motion_validator.cpp src/discrete_motion_validator.cpp + src/state_collision_validator.cpp + src/compound_state_validator.cpp src/weighted_real_vector_state_sampler.cpp - src/ompl_motion_planner_status_category.cpp + # Motion planner src/ompl_planner_configurator.cpp - src/ompl_problem.cpp - src/profile/ompl_default_plan_profile.cpp + src/ompl_motion_planner_status_category.cpp + src/ompl_motion_planner.cpp src/utils.cpp - src/state_collision_validator.cpp - src/compound_state_validator.cpp - src/serialize.cpp - src/deserialize.cpp) + # Profiles + ) # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif() diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h deleted file mode 100644 index 714a509d33f..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h +++ /dev/null @@ -1,154 +0,0 @@ -/** - * @file ompl_problem.h - * @brief Tesseract OMPL problem definition - * - * @author Levi Armstrong - * @date June 18, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLProblem) -%ignore tesseract_planning::OMPLProblem::extractor; -#endif // SWIG - -namespace tesseract_planning -{ -struct OMPLProblem; - -using StateSamplerAllocator = - std::function; - -using OptimizationObjectiveAllocator = - std::function; - -using StateValidityCheckerAllocator = - std::function; - -using MotionValidatorAllocator = - std::function; - -enum class OMPLProblemStateSpace -{ - REAL_STATE_SPACE, -#ifndef OMPL_LESS_1_4_0 - REAL_CONSTRAINTED_STATE_SPACE, -#endif - SE3_STATE_SPACE, -}; - -struct OMPLProblem -{ - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - using UPtr = std::unique_ptr; - using ConstUPtr = std::unique_ptr; - - // These are required for Tesseract to configure ompl - tesseract_environment::Environment::ConstPtr env; - tesseract_scene_graph::SceneState env_state; - - // This is used to verify that start and goal states are not in collision - tesseract_collision::DiscreteContactManager::Ptr contact_checker; - - // Problem Configuration - OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; - - // Kinematic Objects - tesseract_kinematics::JointGroup::ConstPtr manip; - - /** @brief Max planning time allowed in seconds */ - double planning_time = 5.0; - - /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ - int max_solutions = 10; - - /** - * @brief Simplify trajectory. - * - * Note: If set to true it ignores n_output_states and returns the simplest trajectory. - */ - bool simplify = false; - - /** - * @brief Number of states in the output trajectory - * Note: This is ignored if the simplify is set to true. - * Note: The trajectory can be longer if original trajectory is longer and reducing the number of states causes - * the solution to be invalid. - */ - int n_output_states = 20; - - /** - * @brief This uses all available planning time to create the most optimized trajectory given the objective function. - * - * This is required because not all OMPL planners are optimize graph planners. If the planner you choose is an - * optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still - * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based - * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set - * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the - * most optimal solution. - */ - bool optimize = true; - - /** @brief OMPL problem to be solved ***REQUIRED*** */ - ompl::geometric::SimpleSetupPtr simple_setup; - - /** - * @brief The planner configurators ***REQUIRED*** - * - * This will create a new thread for each planner configurator provided. T - */ - std::vector planners; - - /** - * @brief This will extract an Eigen::VectorXd from the OMPL State ***REQUIRED*** - */ - OMPLStateExtractor extractor; - - /** - * @brief Convert the path stored in simple_setup to tesseract trajectory - * This is required because the motion planner is not aware of the state space type. - * @return Tesseract Trajectory - */ - tesseract_common::TrajArray getTrajectory() const; -}; - -} // namespace tesseract_planning - -#ifdef SWIG -%template(OMPLProblems) std::vector>; -#endif - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROBLEM_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h deleted file mode 100644 index 7250905a943..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ /dev/null @@ -1,161 +0,0 @@ -/** - * @file ompl_default_plan_profile.h - * @brief Tesseract OMPL default plan profile - * - * @author Levi Armstrong - * @date June 18, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLDefaultPlanProfile) -%ignore tesseract_planning::OMPLDefaultPlanProfile::allocWeightedRealVectorStateSampler; -#endif // SWIG - -namespace tesseract_planning -{ -/** - * @brief OMPL does not support the concept of multi waypoint planning like descartes and trajopt. Because of this - * every plan instruction will be its a seperate ompl motion plan and therefore planning information is relevent - * for this motion planner in the profile. - */ -class OMPLDefaultPlanProfile : public OMPLPlanProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - OMPLDefaultPlanProfile() = default; - ~OMPLDefaultPlanProfile() override = default; - OMPLDefaultPlanProfile(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile& operator=(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile(OMPLDefaultPlanProfile&&) noexcept = default; - OMPLDefaultPlanProfile& operator=(OMPLDefaultPlanProfile&&) noexcept = default; - OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - - /** @brief The OMPL state space to use when planning */ - OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE }; - - /** @brief Max planning time allowed in seconds */ - double planning_time = 5.0; - - /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ - int max_solutions = 10; - - /** - * @brief Simplify trajectory. - * - * Note: If set to true it ignores n_output_states and returns the simplest trajectory. - */ - bool simplify = false; - - /** - * @brief This uses all available planning time to create the most optimized trajectory given the objective function. - * - * This is required because not all OMPL planners are optimize graph planners. If the planner you choose is an - * optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still - * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based - * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set - * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the - * most optimal solution. - */ - bool optimize = true; - - /** - * @brief The planner configurators - * - * This defaults to two RRTConnectConfigurator - * - * This will create a new thread for each planner configurator provided. T - */ - std::vector planners = { std::make_shared(), - std::make_shared() }; - - /** @brief The collision check configuration */ - tesseract_collision::CollisionCheckConfig collision_check_config; - - /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */ - StateSamplerAllocator state_sampler_allocator; - - /** @brief Set the optimization objective function allocator. Default is to minimize path length */ - OptimizationObjectiveAllocator optimization_objective_allocator; - - /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses - * StateCollisionValidator */ - StateValidityCheckerAllocator svc_allocator; - - /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used - * ContinuousMotionValidator */ - MotionValidatorAllocator mv_allocator; - - void setup(OMPLProblem& prob) const override; - - void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const override; - - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; - -protected: - ompl::base::StateValidityCheckerPtr processStateValidator(OMPLProblem& prob) const; - void processMotionValidator(OMPLProblem& prob, - const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; - void processOptimizationObjective(OMPLProblem& prob) const; -}; -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h deleted file mode 100644 index 277bd0fe10d..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ /dev/null @@ -1,104 +0,0 @@ -/** - * @file ompl_profile.h - * @brief Tesseract OMPL profile - * - * @author Levi Armstrong - * @date June 18, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H -#define TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -#ifdef SWIG -%shared_ptr(tesseract_planning::OMPLPlanProfile) -#endif // SWIG - -namespace tesseract_planning -{ -class OMPLPlanProfile -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - OMPLPlanProfile() = default; - virtual ~OMPLPlanProfile() = default; - OMPLPlanProfile(const OMPLPlanProfile&) = default; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = default; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = default; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = default; - - OMPLPlanProfile(const tinyxml2::XMLElement& xml_element); - - virtual void setup(OMPLProblem& prob) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual void applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& active_links, - int index) const = 0; - - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; -}; - -using OMPLPlanProfileMap = std::unordered_map; - -/** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */ - -} // namespace tesseract_planning - -#ifdef SWIG -%template(OMPLPlanProfileMap) std::unordered_map>; -%tesseract_command_language_add_profile_type(OMPLPlanProfile); -#endif // SWIG - -#endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h deleted file mode 100644 index 5c463067140..00000000000 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h +++ /dev/null @@ -1,43 +0,0 @@ -/** - * @file types.h - * @brief Tesseract OMPL types - * - * @author Levi Armstrong - * @date June 22, 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_MOTION_PLANNERS_OMPL_TYPES_H -#define TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tesseract_planning -{ -using OMPLStateExtractor = std::function(const ompl::base::State*)>; -} -#endif // TESSERACT_MOTION_PLANNERS_OMPL_TYPES_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h index 0794396bd7a..6278d9a9868 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h @@ -34,8 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include -#include +#include namespace tesseract_planning { @@ -72,35 +71,6 @@ void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr, const tesseract_collision::CollisionCheckConfig& collision_check_config); -/** - * @brief For the provided problem check if the state is in collision - * @param prob The OMPL Problem - * @param state The joint state - * @param contact_map Map of contact results. Will be empty if return true - * @return True if in collision otherwise false - */ -bool checkStateInCollision(OMPLProblem& prob, - const Eigen::VectorXd& state, - tesseract_collision::ContactResultMap& contact_map); - -/** - * @brief For the provided problem check if the state is in collision - * @param prob The OMPL Problem - * @param state The joint state - * @return True if in collision otherwise false - */ -bool checkStateInCollision(OMPLProblem& prob, const Eigen::VectorXd& state); - -/** - * @brief Default State sampler which uses the weights information to scale the sampled state. This is use full - * when you state space has mixed units like meters and radian. - * @param space The ompl state space. - * @return OMPL state sampler shared pointer - */ -ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space, - const Eigen::VectorXd& weights, - const Eigen::MatrixX2d& limits); - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H diff --git a/tesseract_motion_planners/ompl/src/ompl_problem.cpp b/tesseract_motion_planners/ompl/src/ompl_problem.cpp deleted file mode 100644 index 236a25a6002..00000000000 --- a/tesseract_motion_planners/ompl/src/ompl_problem.cpp +++ /dev/null @@ -1,38 +0,0 @@ -/** - * @file ompl_problem.h - * @brief Tesseract OMPL problem definition - * - * @author Levi Armstrong - * @date June 18, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -namespace tesseract_planning -{ -tesseract_common::TrajArray OMPLProblem::getTrajectory() const -{ - assert(extractor != nullptr); - return toTrajArray(this->simple_setup->getSolutionPath(), extractor); -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp deleted file mode 100644 index 24d5696b8bf..00000000000 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ /dev/null @@ -1,773 +0,0 @@ -/** - * @file ompl_default_plan_profile.cpp - * @brief - * - * @author Levi Armstrong - * @date June 18, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include - -namespace tesseract_planning -{ -OMPLDefaultPlanProfile::OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* state_space_element = xml_element.FirstChildElement("StateSpace"); - const tinyxml2::XMLElement* planning_time_element = xml_element.FirstChildElement("PlanningTime"); - const tinyxml2::XMLElement* max_solutions_element = xml_element.FirstChildElement("MaxSolutions"); - const tinyxml2::XMLElement* simplify_element = xml_element.FirstChildElement("Simplify"); - const tinyxml2::XMLElement* optimize_element = xml_element.FirstChildElement("Optimize"); - const tinyxml2::XMLElement* planners_element = xml_element.FirstChildElement("Planners"); - // const tinyxml2::XMLElement* collision_check_element = xml_element.FirstChildElement("CollisionCheck"); - // const tinyxml2::XMLElement* collision_continuous_element = xml_element.FirstChildElement("CollisionContinuous"); - // const tinyxml2::XMLElement* collision_safety_margin_element = - // xml_element.FirstChildElement("CollisionSafetyMargin"); - // const tinyxml2::XMLElement* longest_valid_segment_fraction_element = - // xml_element.FirstChildElement("LongestValidSegme" - // "ntFraction"); - // const tinyxml2::XMLElement* longest_valid_segment_length_element = - // xml_element.FirstChildElement("LongestValidSegment" - // "Length"); - - tinyxml2::XMLError status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (state_space_element != nullptr) - { - auto type = static_cast(OMPLProblemStateSpace::REAL_STATE_SPACE); - status = state_space_element->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing StateSpace type attribute."); - - state_space = static_cast(type); - } - - if (planning_time_element != nullptr) - { - std::string planning_time_string; - status = tesseract_common::QueryStringText(planning_time_element, planning_time_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing PlanningTime string"); - - if (!tesseract_common::isNumeric(planning_time_string)) - throw std::runtime_error("OMPLPlanProfile: PlanningTime is not a numeric values."); - - tesseract_common::toNumeric(planning_time_string, planning_time); - } - - if (max_solutions_element != nullptr) - { - std::string max_solutions_string; - status = tesseract_common::QueryStringText(max_solutions_element, max_solutions_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing MaxSolutions string"); - - if (!tesseract_common::isNumeric(max_solutions_string)) - throw std::runtime_error("OMPLPlanProfile: MaxSolutions is not a numeric values."); - - tesseract_common::toNumeric(max_solutions_string, max_solutions); - } - - if (simplify_element != nullptr) - { - status = simplify_element->QueryBoolText(&simplify); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Simplify string"); - } - - if (optimize_element != nullptr) - { - status = optimize_element->QueryBoolText(&optimize); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Optimize string"); - } - - if (planners_element != nullptr) - { - planners.clear(); - for (const tinyxml2::XMLElement* e = planners_element->FirstChildElement("Planner"); e != nullptr; - e = e->NextSiblingElement("Planner")) - { - int type{ 0 }; - status = e->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("OMPLPlanProfile: Error parsing Planner type attribute."); - - switch (type) - { - case static_cast(OMPLPlannerType::SBL): - { - SBLConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::EST): - { - ESTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LBKPIECE1): - { - LBKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BKPIECE1): - { - BKPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::KPIECE1): - { - KPIECE1Configurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::BiTRRT): - { - BiTRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRT): - { - RRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTConnect): - { - RRTConnectConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::RRTstar): - { - RRTstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::TRRT): - { - TRRTConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRM): - { - PRMConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::PRMstar): - { - PRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::LazyPRMstar): - { - LazyPRMstarConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - case static_cast(OMPLPlannerType::SPARS): - { - SPARSConfigurator::ConstPtr ompl_planner = std::make_shared(*e); - planners.push_back(ompl_planner); - break; - } - default: - { - throw std::runtime_error("Unsupported OMPL Planner type"); - } - } - } - } - - /// @todo Update XML - // if (collision_check_element) - // { - // status = collision_check_element->QueryBoolText(&collision_check); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionCheck string"); - // } - - // if (collision_continuous_element) - // { - // status = collision_continuous_element->QueryBoolText(&collision_continuous); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionContinuous string"); - // } - - /// @todo Update XML - // if (collision_safety_margin_element) - // { - // std::string collision_safety_margin_string; - // status = tesseract_common::QueryStringText(collision_safety_margin_element, collision_safety_margin_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing CollisionSafetyMargin string"); - - // if (!tesseract_common::isNumeric(collision_safety_margin_string)) - // throw std::runtime_error("OMPLPlanProfile: CollisionSafetyMargin is not a numeric values."); - - // tesseract_common::toNumeric(collision_safety_margin_string, collision_safety_margin); - // } - - // if (longest_valid_segment_fraction_element) - // { - // std::string longest_valid_segment_fraction_string; - // status = tesseract_common::QueryStringText(longest_valid_segment_fraction_element, - // longest_valid_segment_fraction_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentFraction string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_fraction_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentFraction is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_fraction_string, longest_valid_segment_fraction); - // } - - // if (longest_valid_segment_length_element) - // { - // std::string longest_valid_segment_length_string; - // status = - // tesseract_common::QueryStringText(longest_valid_segment_length_element, - // longest_valid_segment_length_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("OMPLPlanProfile: Error parsing LongestValidSegmentLength string"); - - // if (!tesseract_common::isNumeric(longest_valid_segment_length_string)) - // throw std::runtime_error("OMPLPlanProfile: LongestValidSegmentLength is not a numeric values."); - - // tesseract_common::toNumeric(longest_valid_segment_length_string, longest_valid_segment_length); - // } -} - -void OMPLDefaultPlanProfile::setup(OMPLProblem& prob) const -{ - prob.planners = planners; - prob.planning_time = planning_time; - prob.max_solutions = max_solutions; - prob.simplify = simplify; - prob.optimize = optimize; - - prob.contact_checker->applyContactManagerConfig(collision_check_config.contact_manager_config); - - std::vector joint_names = prob.manip->getJointNames(); - auto dof = static_cast(prob.manip->numJoints()); - auto limits = prob.manip->getLimits().joint_limits; - - if (state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - prob.extractor = [dof](const ompl::base::State* state) -> Eigen::Map { - return tesseract_planning::RealVectorStateSpaceExtractor(state, dof); - }; - -#ifndef OMPL_LESS_1_4_0 - else if (state_space == OMPLProblemStateSpace::REAL_CONSTRAINTED_STATE_SPACE) - prob.extractor = tesseract_planning::ConstrainedStateSpaceExtractor; -#endif - else - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Unsupported configuration!"); - - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - // Construct the OMPL state space for this manipulator - ompl::base::StateSpacePtr state_space_ptr; - - auto rss = std::make_shared(); - for (unsigned i = 0; i < dof; ++i) - rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); - - if (state_sampler_allocator) - { - rss->setStateSamplerAllocator( - [=](const ompl::base::StateSpace* space) { return state_sampler_allocator(space, prob); }); - } - else - { - Eigen::VectorXd weights = Eigen::VectorXd::Ones(dof); - rss->setStateSamplerAllocator( - [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { - return allocWeightedRealVectorStateSampler(state_space, weights, limits); - }); - } - - state_space_ptr = rss; - - // Setup Longest Valid Segment - processLongestValidSegment(state_space_ptr, collision_check_config); - - // Create Simple Setup from state space - prob.simple_setup = std::make_shared(state_space_ptr); - - // Setup state checking functionality - ompl::base::StateValidityCheckerPtr svc_without_collision = processStateValidator(prob); - - // Setup motion validation (i.e. collision checking) - processMotionValidator(prob, svc_without_collision); - - // make sure the planners run until the time limit, and get the best possible solution - processOptimizationObjective(prob); - } -} - -void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - assert(isPlanInstruction(parent_instruction)); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("OMPL, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("OMPL, working_frame is empty!"); - - Eigen::Isometry3d tcp_offset = prob.env->findTCPOffset(mi); - - Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); - - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - /** @todo Need to add Descartes pose sample to ompl profile */ - tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); - tesseract_kinematics::IKSolutions joint_solutions = - std::dynamic_pointer_cast(prob.manip) - ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); - auto goal_states = std::make_shared(prob.simple_setup->getSpaceInformation()); - std::vector contact_map_vec( - static_cast(joint_solutions.size())); - - for (std::size_t i = 0; i < joint_solutions.size(); ++i) - { - Eigen::VectorXd& solution = joint_solutions[i]; - - // Check limits - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - if (!checkStateInCollision(prob, solution, contact_map_vec[i])) - { - { - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - goal_state[j] = solution[static_cast(j)]; - - goal_states->addState(goal_state); - } - - auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( - solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); - for (const auto& rs : redundant_solutions) - { - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - goal_state[j] = rs[static_cast(j)]; - - goal_states->addState(goal_state); - } - } - } - - if (!goal_states->hasStates()) - { - for (std::size_t i = 0; i < contact_map_vec.size(); i++) - for (const auto& contact_vec : contact_map_vec[i]) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + - contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) - .c_str()); - throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits"); - } - prob.simple_setup->setGoal(goal_states); - } -} - -void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& /*parent_instruction*/, - const ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - // Check limits - Eigen::VectorXd solution = joint_waypoint; - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - tesseract_collision::ContactResultMap contact_map; - if (checkStateInCollision(prob, solution, contact_map)) - { - CONSOLE_BRIDGE_logError("In OMPLDefaultPlanProfile: Goal state is in collision"); - for (const auto& contact_vec : contact_map) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) - .c_str()); - } - - ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); - for (unsigned i = 0; i < dof; ++i) - goal_state[i] = joint_waypoint[i]; - - prob.simple_setup->setGoalState(goal_state); - } -} - -void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const Instruction& parent_instruction, - const ManipulatorInfo& manip_info, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - assert(isPlanInstruction(parent_instruction)); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("OMPL, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("OMPL, working_frame is empty!"); - - Eigen::Isometry3d tcp = prob.env->findTCPOffset(mi); - - Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp.inverse(); - - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - /** @todo Need to add Descartes pose sampler to ompl profile */ - /** @todo Need to also provide the seed instruction to use here */ - tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); - tesseract_kinematics::IKSolutions joint_solutions = - std::dynamic_pointer_cast(prob.manip) - ->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(dof)); - bool found_start_state = false; - std::vector contact_map_vec(joint_solutions.size()); - - for (std::size_t i = 0; i < joint_solutions.size(); ++i) - { - Eigen::VectorXd& solution = joint_solutions[i]; - - // Check limits - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state has invalid bounds"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - if (!checkStateInCollision(prob, solution, contact_map_vec[i])) - { - found_start_state = true; - { - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - start_state[j] = solution[static_cast(j)]; - - prob.simple_setup->addStartState(start_state); - } - - auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( - solution, limits.joint_limits, prob.manip->getRedundancyCapableJointIndices()); - for (const auto& rs : redundant_solutions) - { - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned j = 0; j < dof; ++j) - start_state[j] = rs[static_cast(j)]; - - prob.simple_setup->addStartState(start_state); - } - } - } - - if (!found_start_state) - { - for (std::size_t i = 0; i < contact_map_vec.size(); i++) - for (const auto& contact_vec : contact_map_vec[i]) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + - contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) - .c_str()); - throw std::runtime_error("In OMPLPlannerFreespaceConfig: All start states are either in collision or outside " - "limits"); - } - } -} - -void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const Instruction& /*parent_instruction*/, - const ManipulatorInfo& /*manip_info*/, - const std::vector& /*active_links*/, - int /*index*/) const -{ - const auto dof = prob.manip->numJoints(); - tesseract_common::KinematicLimits limits = prob.manip->getLimits(); - - if (prob.state_space == OMPLProblemStateSpace::REAL_STATE_SPACE) - { - Eigen::VectorXd solution = joint_waypoint; - - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state is outside limits"); - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - tesseract_collision::ContactResultMap contact_map; - if (checkStateInCollision(prob, joint_waypoint, contact_map)) - { - CONSOLE_BRIDGE_logError("In OMPLPlannerFreespaceConfig: Start state is in collision"); - for (const auto& contact_vec : contact_map) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) - .c_str()); - } - - ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace()); - for (unsigned i = 0; i < dof; ++i) - start_state[i] = joint_waypoint[i]; - - prob.simple_setup->addStartState(start_state); - } -} - -tinyxml2::XMLElement* OMPLDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const -{ - Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(2).c_str()); - - tinyxml2::XMLElement* xml_ompl = doc.NewElement("OMPLPlanProfile"); - - tinyxml2::XMLElement* xml_ompl_planners = doc.NewElement("Planners"); - - for (const auto& planner : planners) - { - tinyxml2::XMLElement* xml_ompl_planner = doc.NewElement("Planner"); - xml_ompl_planner->SetAttribute("type", std::to_string(static_cast(planner->getType())).c_str()); - tinyxml2::XMLElement* xml_planner = planner->toXML(doc); - xml_ompl_planner->InsertEndChild(xml_planner); - xml_ompl_planners->InsertEndChild(xml_ompl_planner); - } - - xml_ompl->InsertEndChild(xml_ompl_planners); - - tinyxml2::XMLElement* xml_state_space = doc.NewElement("StateSpace"); - xml_state_space->SetAttribute("type", std::to_string(static_cast(state_space)).c_str()); - xml_ompl->InsertEndChild(xml_state_space); - - tinyxml2::XMLElement* xml_planning_time = doc.NewElement("PlanningTime"); - xml_planning_time->SetText(planning_time); - xml_ompl->InsertEndChild(xml_planning_time); - - tinyxml2::XMLElement* xml_max_solutions = doc.NewElement("MaxSolutions"); - xml_max_solutions->SetText(max_solutions); - xml_ompl->InsertEndChild(xml_max_solutions); - - tinyxml2::XMLElement* xml_simplify = doc.NewElement("Simplify"); - xml_simplify->SetText(simplify); - xml_ompl->InsertEndChild(xml_simplify); - - tinyxml2::XMLElement* xml_optimize = doc.NewElement("Optimize"); - xml_optimize->SetText(optimize); - xml_ompl->InsertEndChild(xml_optimize); - - /// @todo Update XML - // tinyxml2::XMLElement* xml_collision_check = doc.NewElement("CollisionCheck"); - // xml_collision_check->SetText(collision_check); - // xml_ompl->InsertEndChild(xml_collision_check); - - // tinyxml2::XMLElement* xml_collision_continuous = doc.NewElement("CollisionContinuous"); - // xml_collision_continuous->SetText(collision_continuous); - // xml_ompl->InsertEndChild(xml_collision_continuous); - - // tinyxml2::XMLElement* xml_collision_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // xml_collision_safety_margin->SetText(collision_safety_margin); - // xml_ompl->InsertEndChild(xml_collision_safety_margin); - - // tinyxml2::XMLElement* xml_long_valid_seg_frac = doc.NewElement("LongestValidSegmentFraction"); - // xml_long_valid_seg_frac->SetText(longest_valid_segment_fraction); - // xml_ompl->InsertEndChild(xml_long_valid_seg_frac); - - // tinyxml2::XMLElement* xml_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - // xml_long_valid_seg_len->SetText(longest_valid_segment_length); - // xml_ompl->InsertEndChild(xml_long_valid_seg_len); - - // TODO: Add plugins for state_sampler_allocator, optimization_objective_allocator, svc_allocator, - // mv_allocator - - xml_planner->InsertEndChild(xml_ompl); - - return xml_planner; -} - -ompl::base::StateValidityCheckerPtr OMPLDefaultPlanProfile::processStateValidator(OMPLProblem& prob) const -{ - ompl::base::StateValidityCheckerPtr svc_without_collision; - auto csvc = std::make_shared(); - if (svc_allocator != nullptr) - { - svc_without_collision = svc_allocator(prob.simple_setup->getSpaceInformation(), prob); - csvc->addStateValidator(svc_without_collision); - } - - if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::DISCRETE || - collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE) - { - auto svc = std::make_shared( - prob.simple_setup->getSpaceInformation(), *prob.env, prob.manip, collision_check_config, prob.extractor); - csvc->addStateValidator(svc); - } - prob.simple_setup->setStateValidityChecker(csvc); - - return svc_without_collision; -} - -void OMPLDefaultPlanProfile::processMotionValidator( - OMPLProblem& prob, - const ompl::base::StateValidityCheckerPtr& svc_without_collision) const -{ - if (mv_allocator != nullptr) - { - auto mv = mv_allocator(prob.simple_setup->getSpaceInformation(), prob); - prob.simple_setup->getSpaceInformation()->setMotionValidator(mv); - } - else - { - if (collision_check_config.type != tesseract_collision::CollisionEvaluatorType::NONE) - { - ompl::base::MotionValidatorPtr mv; - if (collision_check_config.type == tesseract_collision::CollisionEvaluatorType::CONTINUOUS || - collision_check_config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) - { - mv = std::make_shared(prob.simple_setup->getSpaceInformation(), - svc_without_collision, - *prob.env, - prob.manip, - collision_check_config, - prob.extractor); - } - else - { - // Collision checking is preformed using the state validator which this calls. - mv = std::make_shared(prob.simple_setup->getSpaceInformation()); - } - prob.simple_setup->getSpaceInformation()->setMotionValidator(mv); - } - } -} - -void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) const -{ - if (optimization_objective_allocator) - { - prob.simple_setup->getProblemDefinition()->setOptimizationObjective( - optimization_objective_allocator(prob.simple_setup->getSpaceInformation(), prob)); - } - else if (prob.optimize) - { - // Add default optimization function to minimize path length - prob.simple_setup->getProblemDefinition()->setOptimizationObjective( - std::make_shared(prob.simple_setup->getSpaceInformation())); - } -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/utils.cpp b/tesseract_motion_planners/ompl/src/utils.cpp index 17da4838f1e..c560d884b09 100644 --- a/tesseract_motion_planners/ompl/src/utils.cpp +++ b/tesseract_motion_planners/ompl/src/utils.cpp @@ -100,31 +100,4 @@ void processLongestValidSegment(const ompl::base::StateSpacePtr& state_space_ptr state_space_ptr->setLongestValidSegmentFraction(longest_valid_segment_fraction); } -bool checkStateInCollision(OMPLProblem& prob, - const Eigen::VectorXd& state, - tesseract_collision::ContactResultMap& contact_map) -{ - tesseract_common::TransformMap link_transforms = prob.manip->calcFwdKin(state); - - for (const auto& link_name : prob.contact_checker->getActiveCollisionObjects()) - prob.contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); - - prob.contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); - - return (!contact_map.empty()); -} - -bool checkStateInCollision(OMPLProblem& prob, const Eigen::VectorXd& state) -{ - tesseract_collision::ContactResultMap contact_map; - return checkStateInCollision(prob, state, contact_map); -} - -ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base::StateSpace* space, - const Eigen::VectorXd& weights, - const Eigen::MatrixX2d& limits) -{ - return std::make_shared(space, weights, limits); -} - } // namespace tesseract_planning From ca918ede5c38fd1b107a5dae9157ee581737a2c2 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:07:38 -0600 Subject: [PATCH 12/46] Added OMPL planner profile data; updated OMPL motion planner --- .../ompl/ompl_motion_planner.h | 63 +- .../ompl/src/ompl_motion_planner.cpp | 536 ++++++------------ 2 files changed, 205 insertions(+), 394 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 148da795c09..557590d38fc 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -29,27 +29,66 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include +#include #ifdef SWIG %shared_ptr(tesseract_planning::OMPLMotionPlanner) #endif // SWIG -namespace ompl +namespace tesseract_planning { -namespace tools +/** @brief Configuration parameters for the Tesseract OMPL planner */ +struct OMPLPlannerParameters { -class ParallelPlan; -} -} // namespace ompl + /** @brief Max planning time allowed in seconds */ + double planning_time = 5.0; -namespace tesseract_planning + /** @brief The max number of solutions. If max solutions are hit it will exit even if other threads are running. */ + int max_solutions = 10; + + /** + * @brief Simplify trajectory to the minimum required number of states. + * @details Note: trajectory simplification can require a non-trivial amount of time to perform + */ + bool simplify = true; + + /** + * @brief Use all available planning time to create the most optimized trajectory given the objective function. + * @details This is required because not all OMPL planners are optimize graph planners. If the planner you choose is + * an optimize graph planner then setting this to true has no affect. In the case of non-optimize planners they still + * use the OptimizeObjective function but only when searching the graph to find the most optimize solution based + * on the provided optimize objective function. In the case of these type of planners like RRT and RRTConnect if set + * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the + * most optimal solution. + */ + bool optimize = true; + + /** @brief OMPL planning factories for creating OMPL planners to solve the planning problem */ + std::vector planners; +}; + +/** @brief Function signature for extracting a Tesseract state representations from an OMPL state representation */ +using OMPLStateExtractor = std::function(const ompl::base::State*)>; + +/** + * Structure for the output data of an OMPL composite instruction + */ +using OMPLCompositeProfileData = std::tuple; + +/** + * @brief Planner profile that produces the high level parameters for the OMPL planner + */ +struct OMPLPlannerProfile : public PlannerProfile { -class OMPLMotionPlannerStatusCategory; + OMPLPlannerParameters params; + inline OMPLPlannerParameters create() const override { return params; }; +}; /** * @brief This planner is intended to provide an easy to use interface to OMPL for freespace planning. It is made to @@ -94,17 +133,9 @@ class OMPLMotionPlanner : public MotionPlanner static bool checkUserInput(const PlannerRequest& request); - virtual std::vector createProblems(const PlannerRequest& request) const; - protected: /** @brief Name of planner */ std::string name_; - - /** @brief The planners status codes */ - std::shared_ptr status_category_; - - /** @brief OMPL Parallel planner */ - std::shared_ptr parallel_plan_; }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 83dd9032b0d..21c6af8f23d 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -32,18 +32,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include - -#include - -#include #include -#include -#include -#include -#include +#include +#include #include - +#include +#include #include #include @@ -88,7 +82,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, /** @brief Construct a basic planner */ OMPLMotionPlanner::OMPLMotionPlanner(std::string name) - : name_(std::move(name)), status_category_(std::make_shared(name_)) + : name_(std::move(name)) { if (name_.empty()) throw std::runtime_error("OMPLMotionPlanner name is empty!"); @@ -102,433 +96,219 @@ bool OMPLMotionPlanner::terminate() return false; } +void OMPLMotionPlanner::clear() +{ + +} + tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, PlannerResponse& response, bool verbose) const { - if (!checkUserInput(request)) // NOLINT + auto status_category_ = std::make_shared(name_); + + // Check the format of the request + if (!checkUserInput(request)) // NOLINT { response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); return response.status; } - std::vector problem; - if (request.data) - { - problem = *std::static_pointer_cast>(request.data); - } - else - { - try - { - problem = createProblems(request); - } - catch (std::exception& e) - { - CONSOLE_BRIDGE_logError("OMPLPlanner failed to generate problem: %s.", e.what()); - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } - - response.data = std::make_shared>(problem); - } // If the verbose set the log level to debug. if (verbose) console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - /// @todo: Need to expand this to support multiple motion plans leveraging taskflow - for (auto& p : problem) + // Get the planner profile + OMPLPlannerParameters params; + try + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); + auto pp = request.profiles->getProfile>(name_, profile_name); + params = pp->create(); + } + catch (const std::exception& ex) + { + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + + // Get the composite profile + ompl::geometric::SimpleSetupPtr simple_setup; + OMPLStateExtractor extractor; + try + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + auto cp = request.profiles->getProfile>(name_, profile_name); + std::tie(simple_setup, extractor) = cp->create(request.instructions, request.env); + } + catch (const std::exception& ex) { - auto parallel_plan = std::make_shared(p->simple_setup->getProblemDefinition()); + CONSOLE_BRIDGE_logError(ex.what()); + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } - for (const auto& planner : p->planners) - parallel_plan->addPlanner(planner->create(p->simple_setup->getSpaceInformation())); + // Create an OMPL planner that can run multiple OMPL planners in parallele + auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); - ompl::base::PlannerStatus status; - if (!p->optimize) + // Add all the specified OMPL planners to the parallel planner + for (const auto& factory : params.planners) + parallel_plan->addPlanner(factory->create(simple_setup->getSpaceInformation())); + + ompl::base::PlannerStatus status; + if (!params.optimize) + { + // Solve problem. Results are stored in the response + // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state + // and finishes at the end state. + status = parallel_plan->solve(params.planning_time, 1, static_cast(params.max_solutions), false); + } + else + { + ompl::time::point end = ompl::time::now() + ompl::time::seconds(params.planning_time); + const ompl::base::ProblemDefinitionPtr& pdef = simple_setup->getProblemDefinition(); + while (ompl::time::now() < end) { // Solve problem. Results are stored in the response // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state // and finishes at the end state. - status = parallel_plan->solve(p->planning_time, 1, static_cast(p->max_solutions), false); - } - else - { - ompl::time::point end = ompl::time::now() + ompl::time::seconds(p->planning_time); - const ompl::base::ProblemDefinitionPtr& pdef = p->simple_setup->getProblemDefinition(); - while (ompl::time::now() < end) + ompl::base::PlannerStatus localResult = + parallel_plan->solve(std::max(ompl::time::seconds(end - ompl::time::now()), 0.0), + 1, + static_cast(params.max_solutions), + false); + if (localResult) { - // Solve problem. Results are stored in the response - // Disabling hybridization because there is a bug which will return a trajectory that starts at the end state - // and finishes at the end state. - ompl::base::PlannerStatus localResult = - parallel_plan->solve(std::max(ompl::time::seconds(end - ompl::time::now()), 0.0), - 1, - static_cast(p->max_solutions), - false); - if (localResult) + if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) + status = localResult; + + if (!pdef->hasOptimizationObjective()) { - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - status = localResult; - - if (!pdef->hasOptimizationObjective()) - { - CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified"); - break; - } - - ompl::base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); - CONSOLE_BRIDGE_logDebug("Motion Objective Cost: %f", obj_cost.value()); - - if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) - { - CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective"); - break; - } - - if (pdef->getSolutionCount() >= static_cast(p->max_solutions)) - { - CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", p->max_solutions); - break; - } + CONSOLE_BRIDGE_logDebug("Terminating early since there is no optimization objective specified"); + break; } - } - } - if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - { - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, - status_category_); - return response.status; - } + ompl::base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); + CONSOLE_BRIDGE_logDebug("Motion Objective Cost: %f", obj_cost.value()); - if (p->simplify) - { - p->simple_setup->simplifySolution(); - } - else - { - // Interpolate the path if it shouldn't be simplified and there are currently fewer states than requested - auto num_output_states = static_cast(p->n_output_states); - if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states) - { - p->simple_setup->getSolutionPath().interpolate(num_output_states); - } - else - { - // Now try to simplify the trajectory to get it under the requested number of output states - // The interpolate function only executes if the current number of states is less than the requested - p->simple_setup->simplifySolution(); - if (p->simple_setup->getSolutionPath().getStateCount() < num_output_states) - p->simple_setup->getSolutionPath().interpolate(num_output_states); + if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) + { + CONSOLE_BRIDGE_logDebug("Terminating early since solution path satisfies the optimization objective"); + break; + } + + if (pdef->getSolutionCount() >= static_cast(params.max_solutions)) + { + CONSOLE_BRIDGE_logDebug("Terminating early since %u solutions were generated", params.max_solutions); + break; + } } } } - // Flatten the results to make them easier to process - response.results = request.seed; - std::vector> results_flattened = - flattenProgramToPattern(response.results, request.instructions); - std::vector> instructions_flattened = flattenProgram(request.instructions); - - std::size_t instructions_idx = 0; // Index for each input instruction - - // Handle the start instruction - const auto& plan_instruction = instructions_flattened.at(0).get().as(); - if (plan_instruction.isStart()) + if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) { - const auto& p = problem[0]; - - // Get the results - tesseract_common::TrajArray trajectory = p->getTrajectory(); - - // Enforce limits - for (Eigen::Index i = 0; i < trajectory.rows(); i++) - tesseract_common::enforcePositionLimits(trajectory.row(i), p->manip->getLimits().joint_limits); - - assert(checkStartState(p->simple_setup->getProblemDefinition(), trajectory.row(0), p->extractor)); - assert(checkGoalState(p->simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), p->extractor)); - - // Copy the start instruction - assert(instructions_idx == 0); - assert(isMoveInstruction(results_flattened[0].get())); - auto& move_instruction = results_flattened[0].get().as(); - move_instruction.getWaypoint().as().position = trajectory.row(0); - instructions_idx++; + response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, + status_category_); + return response.status; } - // Loop over remaining instructions - std::size_t prob_idx = 0; - for (; instructions_idx < instructions_flattened.size(); instructions_idx++) + // The trajectory must have at least as many states as the seed trajectory + const std::size_t n_states = simple_setup->getSolutionPath().getStateCount(); + if (n_states < request.seed.size()) { - if (isPlanInstruction(instructions_flattened.at(instructions_idx).get())) - { - const auto& p = problem[prob_idx]; - - // Get the results - tesseract_common::TrajArray trajectory = p->getTrajectory(); - - assert(checkStartState(p->simple_setup->getProblemDefinition(), trajectory.row(0), p->extractor)); - assert( - checkGoalState(p->simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), p->extractor)); - - // Loop over the flattened results and add them to response if the input was a plan instruction - auto& move_instructions = results_flattened[instructions_idx].get().as(); - // Adjust result index to align final point since start instruction is already handled - Eigen::Index result_index = trajectory.rows() - static_cast(move_instructions.size()); - for (auto& instruction : move_instructions) - instruction.as().getWaypoint().as().position = trajectory.row(result_index++); - - // Increment the problem - prob_idx++; - } + // Upsample the number of states to match the length of the seed + simple_setup->getSolutionPath().interpolate(static_cast(request.seed.size())); } - - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); - return response.status; -} - -void OMPLMotionPlanner::clear() { parallel_plan_ = nullptr; } - -MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } - -bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) -{ - // Check that parameters are valid - if (request.env == nullptr) + else if (n_states > request.seed.size() && params.simplify) { - CONSOLE_BRIDGE_logError("In TrajOptPlannerUniversalConfig: env is a required parameter and has not been set"); - return false; - } + // Simplify the solution + simple_setup->simplifySolution(); - if (request.instructions.empty()) + // Upsample the number of states to match the length of the seed if required + if (simple_setup->getSolutionPath().getStateCount() < request.seed.size()) + simple_setup->getSolutionPath().interpolate(static_cast(request.seed.size())); + } + else { - CONSOLE_BRIDGE_logError("TrajOptPlannerUniversalConfig requires at least one instruction"); - return false; + // Trajectory length matches seed length, so no need to simplify or interpolate } - return true; -} + // Get the results + tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); + assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); + assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); -OMPLProblem::Ptr createOMPLSubProblem(const PlannerRequest& request, - const tesseract_kinematics::JointGroup::ConstPtr& manip) -{ - auto sub_prob = std::make_unique(); - sub_prob->env = request.env; - sub_prob->env_state = request.env_state; - sub_prob->manip = manip; - sub_prob->contact_checker = request.env->getDiscreteContactManager(); - sub_prob->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); - sub_prob->contact_checker->setActiveCollisionObjects(manip->getActiveLinkNames()); - return sub_prob; -} + // Enforce limits + { + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; + for (Eigen::Index i = 0; i < trajectory.rows(); i++) + tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); + } -std::vector OMPLMotionPlanner::createProblems(const PlannerRequest& request) const -{ - std::vector problem; - tesseract_kinematics::JointGroup::Ptr manip; + // Initialize the output by copying the input program and flattening it + response.results = request.seed; + std::vector> results_flattened = + flattenProgramToPattern(response.results, request.instructions); - // Assume all the plan instructions have the same manipulator as the composite - assert(!request.instructions.getManipulatorInfo().empty()); + // Element 0 of the flattened results is a start instruction that should contain the first trajectory state + auto& start_instruction = results_flattened.at(0).get().as(); - const ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); + // Element 1 of the flattened results is a composite instruction containing the reset of the trajectory + auto& composite = results_flattened.at(1).get().as(); - if (composite_mi.manipulator.empty()) - throw std::runtime_error("OMPL, manipulator is empty!"); + // The results composite will only have as many states as the seed, but the OMPL trajectory might require more states + // In this case, we need to insert more states into the composite to cover the difference + if (composite.size() < trajectory.rows()) + { + const std::size_t diff = static_cast(trajectory.rows()) - composite.size(); + composite.insert(composite.end(), diff, composite.back()); + } - try + // Overwrite the contents of each copied waypoint + for (Eigen::Index i = 0; i < trajectory.rows(); ++i) { - tesseract_kinematics::KinematicGroup::Ptr kin_group; - std::string error_msg; - if (composite_mi.manipulator_ik_solver.empty()) + // The first trajectory state goes into the start instruction + if (i == 0) { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "'"; + start_instruction.getWaypoint().as().position = trajectory.row(i); } else { - kin_group = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - error_msg = "Failed to find kinematic group for manipulator '" + composite_mi.manipulator + "' with solver '" + - composite_mi.manipulator_ik_solver + "'"; - } - - if (kin_group == nullptr) - { - CONSOLE_BRIDGE_logError("%s", error_msg.c_str()); - throw std::runtime_error(error_msg); + // Subsequent trajectory states go into the composite instruction + auto& move_instruction = composite.at(static_cast(i)).as(); + move_instruction.getWaypoint().as().position = trajectory.row(i); } - - manip = kin_group; - } - catch (...) - { - manip = request.env->getJointGroup(composite_mi.manipulator); } - if (!manip) - throw std::runtime_error("Failed to get joint/kinematic group: " + composite_mi.manipulator); + response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); + return response.status; +} - std::vector joint_names = manip->getJointNames(); - std::vector active_link_names = manip->getActiveLinkNames(); +MotionPlanner::Ptr OMPLMotionPlanner::clone() const { return std::make_shared(name_); } - // Check and make sure it does not contain any composite instruction - for (const auto& instruction : request.instructions) - if (isCompositeInstruction(instruction)) - throw std::runtime_error("OMPL planner does not support child composite instructions."); +bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) +{ + // Check that parameters are valid + if (request.env == nullptr) + std::runtime_error("Environment is invalid (nullptr)"); - int index = 0; - Waypoint start_waypoint{ NullWaypoint() }; - Instruction placeholder_instruction{ NullInstruction() }; - const Instruction* start_instruction = nullptr; - if (request.instructions.hasStartInstruction()) - { - assert(isPlanInstruction(request.instructions.getStartInstruction())); - start_instruction = &(request.instructions.getStartInstruction()); - if (isPlanInstruction(*start_instruction)) - { - const auto& temp = start_instruction->as(); - assert(temp.isStart()); - start_waypoint = temp.getWaypoint(); - } - else - { - throw std::runtime_error("OMPL DefaultProblemGenerator: Unsupported start instruction type!"); - } - } - else - { - Eigen::VectorXd current_jv = request.env_state.getJointValues(joint_names); - StateWaypoint swp(joint_names, current_jv); + if (request.instructions.empty()) + std::runtime_error("Request contains no instructions"); - MoveInstruction temp_move(swp, MoveInstructionType::START); - placeholder_instruction = temp_move; - start_instruction = &placeholder_instruction; - start_waypoint = swp; - } + if (request.instructions.size() > 1) + std::runtime_error("OMPL planner only supports one plan instruction"); - // Transform plan instructions into ompl problem - for (std::size_t i = 0; i < request.instructions.size(); ++i) - { - const auto& instruction = request.instructions[i]; - if (isPlanInstruction(instruction)) - { - assert(isPlanInstruction(instruction)); - const auto& plan_instruction = instruction.as(); - - assert(isCompositeInstruction(request.seed[i])); - const auto& seed_composite = request.seed[i].as(); - - // Get Plan Profile - std::string profile = plan_instruction.getProfile(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - auto cur_plan_profile = - getProfile(name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, plan_instruction.profile_overrides); - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Invalid profile"); - - /** @todo Should check that the joint names match the order of the manipulator */ - OMPLProblem::Ptr sub_prob = createOMPLSubProblem(request, manip); - cur_plan_profile->setup(*sub_prob); - sub_prob->n_output_states = static_cast(seed_composite.size()) + 1; - - if (plan_instruction.isLinear()) - { - /** @todo Add support for linear motion to ompl planner */ - if (isCartesianWaypoint(plan_instruction.getWaypoint()) || isJointWaypoint(plan_instruction.getWaypoint()) || - isStateWaypoint(start_waypoint)) - { - // TODO Currently skipping linear moves until SE3 motion planning is implemented. - problem.push_back(nullptr); - ++index; - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); - } - } - else if (plan_instruction.isFreespace()) - { - if (isJointWaypoint(plan_instruction.getWaypoint()) || isStateWaypoint(plan_instruction.getWaypoint())) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->applyGoalStates( - *sub_prob, cur_position, plan_instruction, composite_mi, active_link_names, index); - - ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace()); - if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) - { - assert(checkJointPositionFormat(joint_names, start_waypoint)); - const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint); - cur_plan_profile->applyStartStates( - *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names, index); - } - else if (isCartesianWaypoint(start_waypoint)) - { - const auto& prev_wp = start_waypoint.as(); - cur_plan_profile->applyStartStates( - *sub_prob, prev_wp, *start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); - } - - problem.push_back(std::move(sub_prob)); - ++index; - } - else if (isCartesianWaypoint(plan_instruction.getWaypoint())) - { - const auto& cur_wp = plan_instruction.getWaypoint().as(); - cur_plan_profile->applyGoalStates( - *sub_prob, cur_wp, plan_instruction, composite_mi, active_link_names, index); - - if (index == 0) - { - ompl::base::ScopedState<> start_state(sub_prob->simple_setup->getStateSpace()); - if (isJointWaypoint(start_waypoint) || isStateWaypoint(start_waypoint)) - { - assert(checkJointPositionFormat(joint_names, start_waypoint)); - const Eigen::VectorXd& prev_position = getJointPosition(start_waypoint); - cur_plan_profile->applyStartStates( - *sub_prob, prev_position, *start_instruction, composite_mi, active_link_names, index); - } - else if (isCartesianWaypoint(start_waypoint)) - { - const auto& prev_wp = start_waypoint.as(); - cur_plan_profile->applyStartStates( - *sub_prob, prev_wp, *start_instruction, composite_mi, active_link_names, index); - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: unknown waypoint type"); - } - } - else - { - /** @todo Update. Extract the solution for the previous plan and set as the start */ - assert(false); - } - - problem.push_back(std::move(sub_prob)); - ++index; - } - } - else - { - throw std::runtime_error("OMPLMotionPlannerDefaultConfig: Unsupported!"); - } + // Attempt to cast the first child instruction to a PlanInstruction + request.instructions.at(0).as(); - start_waypoint = plan_instruction.getWaypoint(); /** @todo need to extract the solution for Cartesian waypoints - to work correctly*/ - start_instruction = &instruction; - } - } - - return problem; + return true; } } // namespace tesseract_planning From 4d3070238ea82c6068a017b475d23f7900d662f2 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:12:29 -0600 Subject: [PATCH 13/46] Transitioned contents of OMPL problem and problem generator into a new OMPL composite profile for real value state spaces --- tesseract_motion_planners/ompl/CMakeLists.txt | 2 +- .../profile/ompl_composite_profile_rvss.h | 78 ++++ .../profile/ompl_composite_profile_rvss.cpp | 360 ++++++++++++++++++ 3 files changed, 439 insertions(+), 1 deletion(-) create mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h create mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index 11edc1406f9..cf761e51475 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -16,7 +16,7 @@ set(OMPL_SRC src/ompl_motion_planner.cpp src/utils.cpp # Profiles - ) + src/profile/ompl_composite_profile_rvss.cpp) # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif() diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h new file mode 100644 index 00000000000..a98c06fbd0a --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h @@ -0,0 +1,78 @@ +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_H +#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_planning +{ +using StateValidityCheckerAllocator = + std::function; + +using MotionValidatorAllocator = + std::function; + +using OptimizationObjectiveAllocator = + std::function; + +/** + * @brief OMPL composite profile for real-vector state spaces + */ +struct OMPLCompositeProfileRVSS : public CompositeProfile +{ + /** + * @brief The collision check configuration + * @details Different validators will be created depending on the type of collision checking requested + * - Discrete: + * - Discrete collision state validator, discrete motion validator + * - Continuous: + * - Continuous motion validator + * - None: + * - No additional validators will be created + */ + tesseract_collision::CollisionCheckConfig collision_check_config; + + /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */ + ompl::base::StateSamplerAllocator state_sampler_allocator{ nullptr }; + + /** + * @brief The ompl state validity checker + * @details If nullptr and collision checking enabled it uses StateCollisionValidator + */ + StateValidityCheckerAllocator state_validator_allocator{ nullptr }; + + /** + * @brief The ompl motion validator. + * @details If nullptr and continuous collision checking enabled it used ContinuousMotionValidator + */ + MotionValidatorAllocator motion_validator_allocator{ nullptr }; + + /** + * @brief Function for allocating the optimization objective + * @details Default is the minimization of path length + */ + OptimizationObjectiveAllocator optimization_objective_allocator = [](const ompl::base::SpaceInformationPtr& si) { + return std::make_shared(si); + }; + + /** + * @brief Creates the OMPL composite profile data given a composite instruction and environment + */ + OMPLCompositeProfileData create(const CompositeInstruction& instruction, + tesseract_environment::Environment::ConstPtr env) const override; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_COMPOSITE_PROFILE_RVSS_H diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp new file mode 100644 index 00000000000..501982feb84 --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -0,0 +1,360 @@ +#include + +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +tesseract_collision::ContactResultMap checkCollision(const Eigen::VectorXd& state, + tesseract_kinematics::JointGroup::ConstPtr manip, + tesseract_collision::DiscreteContactManager::Ptr contact_checker) +{ + tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); + + for (const auto& link_name : contact_checker->getActiveCollisionObjects()) + contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); + + tesseract_collision::ContactResultMap contact_map; + contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); + + return contact_map; +} + +std::vector> createOMPLStates(const tesseract_kinematics::IKSolutions& joint_states, + ompl::base::SpaceInformationPtr si) +{ + std::vector> states; + states.reserve(joint_states.size()); + + for (const auto& js : joint_states) + { + ompl::base::ScopedState<> state(si->getStateSpace()); + for (unsigned i = 0; i < js.size(); ++i) + state[i] = js[static_cast(i)]; + + states.push_back(state); + } + + return states; +} + +tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& cartesian_waypoint, + const ManipulatorInfo& mi, + tesseract_environment::Environment::ConstPtr env) +{ + if (mi.manipulator.empty()) + throw std::runtime_error("OMPL: manipulator is empty!"); + + if (mi.tcp_frame.empty()) + throw std::runtime_error("OMPL: tcp_frame is empty!"); + + if (mi.working_frame.empty()) + throw std::runtime_error("OMPL: working_frame is empty!"); + + // Get the kinematics group for solving IK + tesseract_kinematics::KinematicGroup::ConstPtr manip = + env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); + + /** Solve IK + * @todo Need to add Descartes pose sample to ompl profile + */ + Eigen::Isometry3d tcp_offset = env->findTCPOffset(mi); + Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); + const tesseract_kinematics::IKSolutions joint_solutions = + manip->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(manip->numJoints())); + + // Check collision + std::vector contact_map_vec(static_cast(joint_solutions.size())); + tesseract_common::KinematicLimits limits = manip->getLimits(); + tesseract_kinematics::IKSolutions valid_solutions; + valid_solutions.reserve(joint_solutions.size()); + for (std::size_t i = 0; i < joint_solutions.size(); ++i) + { + Eigen::VectorXd solution = joint_solutions[i]; + + // Check limits + if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) + { + tesseract_common::enforcePositionLimits(solution, limits.joint_limits); + } + else + { + CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); + continue; + } + + // Get discrete contact manager for testing provided start and end position + // This is required because collision checking happens in motion validators now + // instead of the isValid function to avoid unnecessary collision checks. + contact_map_vec[i] = checkCollision(solution, manip, env->getDiscreteContactManager()); + if (contact_map_vec[i].empty()) + { + valid_solutions.push_back(solution); + + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, manip->getRedundancyCapableJointIndices()); + + valid_solutions.insert(valid_solutions.end(), redundant_solutions.begin(), redundant_solutions.end()); + } + } + + if (valid_solutions.empty()) + { + for (std::size_t i = 0; i < contact_map_vec.size(); i++) + for (const auto& contact_vec : contact_map_vec[i]) + for (const auto& contact : contact_vec.second) + CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + + contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) + .c_str()); + throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits"); + } + + return valid_solutions; +} + +ompl::base::ScopedState<> createState(Eigen::VectorXd joint_waypoint, + tesseract_environment::Environment::ConstPtr env, + tesseract_kinematics::JointGroup::ConstPtr manip, + ompl::base::StateSpacePtr ss) +{ + const auto dof = manip->numJoints(); + tesseract_common::KinematicLimits limits = manip->getLimits(); + + if (tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) + tesseract_common::enforcePositionLimits(joint_waypoint, limits.joint_limits); + else + throw std::runtime_error("Start state is outside limits"); + + // Get discrete contact manager for testing provided start and end position + // This is required because collision checking happens in motion validators now + // instead of the isValid function to avoid unnecessary collision checks. + tesseract_collision::ContactResultMap contact_map = + checkCollision(joint_waypoint, manip, env->getDiscreteContactManager()); + if (!contact_map.empty()) + { + std::stringstream ss; + ss << "Start state is in collision"; + for (const auto& contact_vec : contact_map) + for (const auto& contact : contact_vec.second) + ss << "\n\tLinks: " << contact.link_names[0] << ", " << contact.link_names[1] + << " Distance: " << contact.distance; + throw std::runtime_error(ss.str()); + } + + ompl::base::ScopedState<> start_state(ss); + for (unsigned i = 0; i < dof; ++i) + start_state[i] = joint_waypoint[i]; + + return start_state; +} + +OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruction& instruction, + tesseract_environment::Environment::ConstPtr env) const +{ + if (instruction.size() > 1) + throw std::runtime_error("The composite instruction contains more than one instruction. The composite instruction " + "can " + "only contain one child and it must be a PlanInstruction type"); + + const PlanInstruction& goal_instruction = instruction.at(0).as(); + if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) + throw std::runtime_error("Move types other than freespace are not currently supported"); + + // Get the joint group information from the composite instruction and environment + const tesseract_kinematics::JointGroup::ConstPtr joint_group = + env->getJointGroup(instruction.getManipulatorInfo().manipulator); + const std::vector joint_names = joint_group->getJointNames(); + const Eigen::MatrixX2d limits = joint_group->getLimits().joint_limits; + const Eigen::Index dof = joint_group->numJoints(); + + // Set up the extractor + OMPLStateExtractor extractor = [dof](const ompl::base::State* state) -> Eigen::Map { + return tesseract_planning::RealVectorStateSpaceExtractor(state, static_cast(dof)); + }; + + // Construct the real vector state space + auto rss = std::make_shared(); + for (unsigned i = 0; i < dof; ++i) + { + rss->addDimension(joint_names[i], limits(i, 0), limits(i, 1)); + } + + // Create the state sampler + if (state_sampler_allocator) + { + rss->setStateSamplerAllocator(state_sampler_allocator); + } + else + { + Eigen::VectorXd weights = Eigen::VectorXd::Ones(dof); + rss->setStateSamplerAllocator( + [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { + return std::make_shared(state_space, weights, limits); + }); + } + + // Setup Longest Valid Segment + processLongestValidSegment(rss, collision_check_config); + + // Create the SimpleSetup from the state space + auto simple_setup = std::make_shared(rss); + + // Setup state checking functionality + ompl::base::StateValidityCheckerPtr custom_validity_checker{ nullptr }; + { + auto csvc = std::make_shared(); + if (state_validator_allocator != nullptr) + { + custom_validity_checker = state_validator_allocator(simple_setup->getSpaceInformation()); + csvc->addStateValidator(custom_validity_checker); + } + else + { + switch (collision_check_config.type) + { + case tesseract_collision::CollisionEvaluatorType::DISCRETE: + case tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE: + { + auto svc = std::make_shared( + simple_setup->getSpaceInformation(), *env, joint_group, collision_check_config, extractor); + csvc->addStateValidator(svc); + break; + } + default: + CONSOLE_BRIDGE_logDebug("DOING NOTHING HERE"); + } + simple_setup->setStateValidityChecker(csvc); + } + } + + // Setup motion validation (i.e. collision checking) + if (motion_validator_allocator != nullptr) + { + simple_setup->getSpaceInformation()->setMotionValidator( + motion_validator_allocator(simple_setup->getSpaceInformation())); + } + else + { + ompl::base::MotionValidatorPtr mv{ nullptr }; + + switch (collision_check_config.type) + { + case tesseract_collision::CollisionEvaluatorType::DISCRETE: + case tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE: + { + // Collision checking is preformed using the state validator which this calls. + mv = std::make_shared(simple_setup->getSpaceInformation()); + break; + } + case tesseract_collision::CollisionEvaluatorType::CONTINUOUS: + case tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS: + { + mv = std::make_shared(simple_setup->getSpaceInformation(), + custom_validity_checker, + *env, + joint_group, + collision_check_config, + extractor); + break; + } + default: + CONSOLE_BRIDGE_logDebug("Nothing doing"); + } + + simple_setup->getSpaceInformation()->setMotionValidator(mv); + } + + // make sure the planners run until the time limit, and get the best possible solution + if (optimization_objective_allocator) + { + simple_setup->getProblemDefinition()->setOptimizationObjective( + optimization_objective_allocator(simple_setup->getSpaceInformation())); + } + + // Add the start waypoint + { + // Get the start waypoint + Waypoint start_waypoint{ NullWaypoint() }; + tesseract_common::ManipulatorInfo start_mi = instruction.getManipulatorInfo(); + try + { + const auto& pi = instruction.getStartInstruction().as(); + start_waypoint = pi.getWaypoint(); + start_mi = start_mi.getCombined(pi.getManipulatorInfo()); + } + catch (const std::bad_cast& ex) + { + Eigen::VectorXd current_jv = env->getCurrentJointValues(); + start_waypoint = StateWaypoint(joint_names, current_jv); + } + + // Add the start state(s) + if (isCartesianWaypoint(start_waypoint)) + { + const CartesianWaypoint& cw = start_waypoint.as(); + tesseract_kinematics::IKSolutions sols = getValidIKSolutions(cw, start_mi, env); + auto states = createOMPLStates(sols, simple_setup->getSpaceInformation()); + + // Add the states to the SimpleSetup + std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { + simple_setup->addStartState(state); + }); + } + else if (isJointWaypoint(start_waypoint)) + { + const JointWaypoint& jw = start_waypoint.as(); + simple_setup->addStartState(createState(jw, env, joint_group, simple_setup->getStateSpace())); + } + else + { + throw std::runtime_error("Unsupported start waypoint type"); + } + } + + // Add the goal waypoint + { + tesseract_common::ManipulatorInfo goal_mi = + goal_instruction.getManipulatorInfo().getCombined(instruction.getManipulatorInfo()); + + // Add the goal state + if (isCartesianWaypoint(goal_instruction.getWaypoint())) + { + const CartesianWaypoint& cw = goal_instruction.getWaypoint().as(); + tesseract_kinematics::IKSolutions sols = getValidIKSolutions(cw, goal_mi, env); + auto states = createOMPLStates(sols, simple_setup->getSpaceInformation()); + + auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); + std::for_each(states.begin(), states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { + goal_states->addState(state); + }); + + simple_setup->setGoal(goal_states); + } + else if (isJointWaypoint(goal_instruction.getWaypoint())) + { + const JointWaypoint& jw = goal_instruction.getWaypoint().as(); + simple_setup->setGoalState(createState(jw, env, joint_group, simple_setup->getStateSpace())); + } + else + { + throw std::runtime_error("Unsupported goal waypoint type"); + } + } + + // Return the composite data + return std::make_tuple(simple_setup, extractor); +} + +} // namespace tesseract_planning From 7292e561b1f5b9dfdc86093c87336a6b8956b7d4 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:13:57 -0600 Subject: [PATCH 14/46] Updated weighted real value state space to accept random seed --- .../ompl/weighted_real_vector_state_sampler.h | 10 +++++++++- .../ompl/src/weighted_real_vector_state_sampler.cpp | 5 ++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h index 80a43789160..a84cb01162d 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h @@ -48,9 +48,17 @@ namespace tesseract_planning class WeightedRealVectorStateSampler : public ompl::base::StateSampler { public: + /** + * @brief Constructor + * @param space + * @param weights + * @param bounds + * @param rng_seed Seed for the OMPL random number generator to produce deterministic sampling. The seed is only set if the input value is greater than greater than or equal to zero + */ WeightedRealVectorStateSampler(const ompl::base::StateSpace* space, const Eigen::Ref& weights, - const Eigen::Ref& bounds); + const Eigen::Ref& bounds, + long rng_seed = -1); void sampleUniform(ompl::base::State* state) override; diff --git a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp index 9694ffcddd9..a02c0d1a439 100644 --- a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp +++ b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp @@ -39,10 +39,13 @@ namespace tesseract_planning { WeightedRealVectorStateSampler::WeightedRealVectorStateSampler(const ompl::base::StateSpace* space, const Eigen::Ref& weights, - const Eigen::Ref& bounds) + const Eigen::Ref& bounds, + long rng_seed) : ompl::base::StateSampler(space), weights_(weights), bounds_(bounds) { assert(bounds_.rows() == weights_.size()); + if(rng_seed >= 0) + rng_.setLocalSeed(static_cast(rng_seed)); } void WeightedRealVectorStateSampler::sampleUniform(ompl::base::State* state) From c5a534d4b44662eb249c8cebbc644e5bd6f2f2e1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:14:31 -0600 Subject: [PATCH 15/46] Added random seed parameter to OMPL RVSS composite profile --- .../ompl/profile/ompl_composite_profile_rvss.h | 6 ++++++ .../ompl/src/profile/ompl_composite_profile_rvss.cpp | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h index a98c06fbd0a..1d537f5cf33 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h @@ -71,6 +71,12 @@ struct OMPLCompositeProfileRVSS : public CompositeProfilesetStateSamplerAllocator( - [weights, limits](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { - return std::make_shared(state_space, weights, limits); + [weights, limits, this](const ompl::base::StateSpace* state_space) -> ompl::base::StateSamplerPtr { + return std::make_shared(state_space, weights, limits, this->rng_seed); }); } From 4d60b0362655768c01a9fa73b9f9dbd597930862 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:14:45 -0600 Subject: [PATCH 16/46] Updated OMPL unit tests --- .../test/ompl_planner_tests.cpp | 477 +++++++++--------- 1 file changed, 227 insertions(+), 250 deletions(-) diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 864f23bb1c4..5dcb22b2d40 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -26,20 +26,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include #include @@ -53,7 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include @@ -69,9 +55,7 @@ using namespace tesseract_kinematics; using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; -const static int SEED = 1; -const static std::vector start_state = { -0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; -const static std::vector end_state = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; +const static long SEED = 1; std::string locateResource(const std::string& url) { @@ -127,10 +111,63 @@ template class OMPLTestFixture : public ::testing::Test { public: - OMPLTestFixture() { configurator = std::make_shared(); } using ::testing::Test::Test; - std::shared_ptr configurator; - OMPLMotionPlanner ompl_planner; + + OMPLTestFixture() + : env(std::make_shared()) + , manip("manipulator", "base_link", "tool0") + , configurator(std::make_shared()) + { + // Load scene and srdf + auto locator = std::make_shared(locateResource); + tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); + tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); + EXPECT_TRUE(this->env->init(urdf_path, srdf_path, locator)); + + // Add box to environment + addBox(*env); + } + + std::shared_ptr createCompositeProfile() + { + auto composite_profile = std::make_shared(); + composite_profile->collision_check_config.contact_manager_config.margin_data_override_type = + tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; + composite_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); + composite_profile->collision_check_config.longest_valid_segment_length = 0.1; + composite_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; + composite_profile->rng_seed = SEED; + + return composite_profile; + } + + std::shared_ptr createPlannerProfile() + { + auto planner_profile = std::make_shared(); + planner_profile->params.planning_time = 10.0; + planner_profile->params.optimize = false; + planner_profile->params.max_solutions = 2; + planner_profile->params.simplify = false; + planner_profile->params.planners = { this->configurator, this->configurator }; + return planner_profile; + } + + /** @brief Motion planning environment */ + Environment::Ptr env; + /** @brief Manipulator information for planning */ + const ManipulatorInfo manip; + /** @brief OMPL planner factory */ + const std::shared_ptr configurator; + /** @brief Tesseract OMPL motion planner */ + const OMPLMotionPlanner ompl_planner; + /** @brief Arbitrary trajectory start state */ + const std::vector start_state_ = { -0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + /** @brief Arbitrary trajectory end state */ + const std::vector end_state_ = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + /** @brief Motion planning profile name */ + const std::string profile_name_ = "TEST_PROFILE"; + /** @brief Number of states in the seed trajectory */ + const int seed_size_ = 10; }; using Implementations = ::testing::Types(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - ManipulatorInfo manip; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - manip.tcp_frame = "tool0"; - - // Step 2: Add box to environment - addBox(*env); - - // Step 3: Create ompl planner config and populate it - auto joint_group = env->getJointGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - JointWaypoint wp1(joint_group->getJointNames(), - Eigen::Map(start_state.data(), static_cast(start_state.size()))); + const JointWaypoint wp1( + joint_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); // Specify a end waypoint - JointWaypoint wp2(joint_group->getJointNames(), - Eigen::Map(end_state.data(), static_cast(end_state.size()))); + const JointWaypoint wp2( + joint_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); - PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - program.push_back(plan_f2); - - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - auto plan_profile = std::make_shared(); - plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = - tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; - plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); - plan_profile->collision_check_config.longest_valid_segment_length = 0.1; - plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); + ASSERT_TRUE(&status); EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 21); // 10 per segment + start a instruction - EXPECT_EQ(planner_response.results.size(), 2); + EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); + EXPECT_EQ(planner_response.results.size(), 1); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); EXPECT_TRUE(wp2.isApprox( getJointPosition( getLastMoveInstruction(planner_response.results.front().as())->getWaypoint()), 1e-5)); - EXPECT_TRUE(wp1.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); +} - // Check for start state in collision error - std::vector swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; +TYPED_TEST(OMPLTestFixture, StartStateInCollision) +{ + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); - // Define New Start Instruction - wp1 = Eigen::Map(swp.data(), static_cast(swp.size())); - wp1.joint_names = joint_group->getJointNames(); + // Check for start state in collision error + const std::vector swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; + const JointWaypoint wp1(joint_group->getJointNames(), + Eigen::Map(swp.data(), static_cast(swp.size()))); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); - start_instruction = PlanInstruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + // Specify a end waypoint + const JointWaypoint wp2( + joint_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); // Create a new program - program = CompositeInstruction(); + CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a new seed - seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - // Update Configuration + PlannerRequest request; request.instructions = program; - request.seed = seed; - request.data = nullptr; // Note: Nust clear the saved problem or it will use it instead. + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.env = this->env; + request.env_state = cur_state; + + // Add the profiles + { + auto d = std::make_unique(); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + request.profiles = std::move(d); + } // Solve - status = ompl_planner.solve(request, planner_response); + PlannerResponse planner_response; + auto status = this->ompl_planner.solve(request, planner_response); EXPECT_FALSE(status); +} - // Check for end state in collision error - swp = start_state; - std::vector ewp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; - - wp1 = Eigen::Map(swp.data(), static_cast(swp.size())); - wp1.joint_names = joint_group->getJointNames(); - - wp2 = Eigen::Map(ewp.data(), static_cast(ewp.size())); - wp2.joint_names = joint_group->getJointNames(); +TYPED_TEST(OMPLTestFixture, EndStateInCollision) +{ + const auto joint_group = this->env->getJointGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); - // Define Start Instruction - start_instruction = PlanInstruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + // Specify a start waypoint + const JointWaypoint wp1( + joint_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); - // Define Plan Instructions - plan_f1 = PlanInstruction(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + // Check for end state in collision error + const std::vector ewp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; + const JointWaypoint wp2(joint_group->getJointNames(), + Eigen::Map(ewp.data(), static_cast(ewp.size()))); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); // Create a new program - program = CompositeInstruction(); + CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a new seed - seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Update Configuration + PlannerRequest request; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); request.instructions = program; - request.seed = seed; - request.data = nullptr; // Note: Nust clear the saved problem or it will use it instead. + request.env = this->env; + request.env_state = cur_state; + + // Add the profiles + { + auto d = std::make_unique(); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + request.profiles = std::move(d); + } // Set new configuration and solve - status = ompl_planner.solve(request, planner_response); + PlannerResponse planner_response; + auto status = this->ompl_planner.solve(request, planner_response); EXPECT_FALSE(status); } -TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) +TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) { - EXPECT_EQ(ompl::RNG::getSeed(), SEED) << "Randomization seed does not match expected: " << ompl::RNG::getSeed() - << " vs. " << SEED; - - // Step 1: Load scene and srdf - auto locator = std::make_shared(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - // Set manipulator - ManipulatorInfo manip; - manip.tcp_frame = "tool0"; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - - // Step 2: Add box to environment - addBox(*(env)); - - // Step 3: Create ompl planner config and populate it - auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto kin_group = this->env->getKinematicGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - JointWaypoint wp1(kin_group->getJointNames(), - Eigen::Map(start_state.data(), static_cast(start_state.size()))); + const JointWaypoint wp1( + kin_group->getJointNames(), + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); // Specify a end waypoint - auto goal_jv = Eigen::Map(end_state.data(), static_cast(end_state.size())); - Eigen::Isometry3d goal = kin_group->calcFwdKin(goal_jv).at(manip.tcp_frame); - CartesianWaypoint wp2 = goal; + const auto goal_jv = + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size())); + const Eigen::Isometry3d goal = kin_group->calcFwdKin(goal_jv).at(this->manip.tcp_frame); + const CartesianWaypoint wp2 = goal; // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - auto plan_profile = std::make_shared(); - plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = - tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; - plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.02); - plan_profile->collision_check_config.longest_valid_segment_length = 0.1; - plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 11); + ASSERT_TRUE(&status); + ASSERT_TRUE(planner_response.results.hasStartInstruction()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); - Eigen::Isometry3d check_goal = - kin_group->calcFwdKin(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint())) - .at(manip.tcp_frame); + const MoveInstruction* last_move = getLastMoveInstruction(planner_response.results); + const Eigen::VectorXd& last_move_joints = getJointPosition(last_move->getWaypoint()); + const Eigen::Isometry3d check_goal = kin_group->calcFwdKin(last_move_joints).at(this->manip.tcp_frame); EXPECT_TRUE(wp2.isApprox(check_goal, 1e-3)); } -TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) +TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) { - EXPECT_EQ(ompl::RNG::getSeed(), SEED) << "Randomization seed does not match expected: " << ompl::RNG::getSeed() - << " vs. " << SEED; - - // Step 1: Load scene and srdf - auto locator = std::make_shared(locateResource); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf"); - tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf"); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - - // Set manipulator - ManipulatorInfo manip; - manip.tcp_frame = "tool0"; - manip.manipulator = "manipulator"; - manip.working_frame = "base_link"; - - // Step 2: Add box to environment - addBox(*(env)); - - // Step 3: Create ompl planner config and populate it - auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); + const auto kin_group = this->env->getKinematicGroup(this->manip.manipulator); + const auto cur_state = this->env->getState(); // Specify a start waypoint - auto start_jv = Eigen::Map(start_state.data(), static_cast(start_state.size())); - Eigen::Isometry3d start = kin_group->calcFwdKin(start_jv).at(manip.tcp_frame); - CartesianWaypoint wp1 = start; + const auto start_jv = + Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size())); + const Eigen::Isometry3d start = kin_group->calcFwdKin(start_jv).at(this->manip.tcp_frame); + const CartesianWaypoint wp1 = start; // Specify a end waypoint - JointWaypoint wp2(kin_group->getJointNames(), - Eigen::Map(end_state.data(), static_cast(end_state.size()))); + const JointWaypoint wp2( + kin_group->getJointNames(), + Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START, "TEST_PROFILE"); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); // Define Plan Instructions - PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, "TEST_PROFILE"); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); // Create a program CompositeInstruction program; + program.setProfile(this->profile_name_); program.setStartInstruction(start_instruction); - program.setManipulatorInfo(manip); + program.setManipulatorInfo(this->manip); program.push_back(plan_f1); - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env, 3.14, 1.0, 3.14, 10); - - // Create Profiles - auto plan_profile = std::make_shared(); - plan_profile->collision_check_config.contact_manager_config.margin_data_override_type = - tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; - plan_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.02); - plan_profile->collision_check_config.longest_valid_segment_length = 0.1; - plan_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - plan_profile->planning_time = 10; - plan_profile->optimize = false; - plan_profile->max_solutions = 2; - plan_profile->simplify = false; - plan_profile->planners = { this->configurator, this->configurator }; - - // Profile Dictionary - auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); - // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = seed; - request.env = env; + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.env = this->env; request.env_state = cur_state; - request.profiles = profiles; - // Create Planner and solve - OMPLMotionPlanner ompl_planner; + // Add the profiles + { + auto d = std::make_unique(); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); + d->addProfile>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + request.profiles = std::move(d); + } + PlannerResponse planner_response; - auto status = ompl_planner.solve(request, planner_response); + auto status = this->ompl_planner.solve(request, planner_response); if (!status) { CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); } - EXPECT_TRUE(&status); - EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_EQ(getMoveInstructionCount(planner_response.results), 11); - EXPECT_TRUE(wp2.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); + ASSERT_TRUE(&status); + ASSERT_TRUE(planner_response.results.hasStartInstruction()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); + + const MoveInstruction* last_mi = getLastMoveInstruction(planner_response.results); + EXPECT_TRUE(wp2.isApprox(getJointPosition(last_mi->getWaypoint()), 1e-5)); - Eigen::Isometry3d check_start = - kin_group->calcFwdKin(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint())) - .at(manip.tcp_frame); + const MoveInstruction* first_mi = getFirstMoveInstruction(planner_response.results); + const Eigen::VectorXd& joints = getJointPosition(first_mi->getWaypoint()); + const Eigen::Isometry3d check_start = kin_group->calcFwdKin(joints).at(this->manip.tcp_frame); EXPECT_TRUE(wp1.isApprox(check_start, 1e-3)); } @@ -512,7 +491,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // addBox(*(env)); // // Step 3: Create ompl planner config and populate it -// auto kin = env->getManipulatorManager()->getFwdKinematicSolver(manip.manipulator); +// auto kin = this->env->getManipulatorManager()->getFwdKinematicSolver(manip.manipulator); // std::vector swp = start_state; // std::vector ewp = end_state; @@ -531,7 +510,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // ompl_config->collision_continuous = true; // ompl_config->collision_check = true; // ompl_config->simplify = false; -// ompl_config->n_output_states = 50; // // Set the planner configuration // ompl_planner.setConfiguration(ompl_config); @@ -544,7 +522,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // CONSOLE_BRIDGE_logError("CI Error: %s", status.message().c_str()); // } // EXPECT_TRUE(&status); -// EXPECT_EQ(ompl_planning_response.joint_trajectory.trajectory.rows(), ompl_config->n_output_states); +// EXPECT_GE(ompl_planning_response.joint_trajectory.trajectory.rows(), request.seed.size()); // // Check for start state in collision error // swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; @@ -580,8 +558,7 @@ int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - // Set the randomization seed for the planners to get repeatable results - ompl::RNG::setSeed(SEED); + ompl::RNG::setSeed(static_cast(SEED)); return RUN_ALL_TESTS(); } From 7ced8e62e668e53e69f8f8ea24395c4786a641b4 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 11:17:02 -0600 Subject: [PATCH 17/46] Clang format --- .../ompl/profile/ompl_composite_profile_rvss.h | 10 +++++----- .../ompl/weighted_real_vector_state_sampler.h | 3 ++- .../ompl/src/ompl_motion_planner.cpp | 14 +++++--------- .../src/profile/ompl_composite_profile_rvss.cpp | 2 +- .../src/weighted_real_vector_state_sampler.cpp | 2 +- 5 files changed, 14 insertions(+), 17 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h index 1d537f5cf33..899923aaa00 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h @@ -20,8 +20,7 @@ namespace tesseract_planning using StateValidityCheckerAllocator = std::function; -using MotionValidatorAllocator = - std::function; +using MotionValidatorAllocator = std::function; using OptimizationObjectiveAllocator = std::function; @@ -74,11 +73,12 @@ struct OMPLCompositeProfileRVSS : public CompositeProfile& weights, diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 21c6af8f23d..4fe389a240f 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -81,8 +81,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, } /** @brief Construct a basic planner */ -OMPLMotionPlanner::OMPLMotionPlanner(std::string name) - : name_(std::move(name)) +OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : name_(std::move(name)) { if (name_.empty()) throw std::runtime_error("OMPLMotionPlanner name is empty!"); @@ -96,10 +95,7 @@ bool OMPLMotionPlanner::terminate() return false; } -void OMPLMotionPlanner::clear() -{ - -} +void OMPLMotionPlanner::clear() {} tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, PlannerResponse& response, @@ -108,7 +104,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ auto status_category_ = std::make_shared(name_); // Check the format of the request - if (!checkUserInput(request)) // NOLINT + if (!checkUserInput(request)) // NOLINT { response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); @@ -213,8 +209,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) { - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, - status_category_); + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, status_category_); return response.status; } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index 598bb07c07b..d4750ee6982 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -357,4 +357,4 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc return std::make_tuple(simple_setup, extractor); } -} // namespace tesseract_planning +} // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp index a02c0d1a439..98872e10489 100644 --- a/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp +++ b/tesseract_motion_planners/ompl/src/weighted_real_vector_state_sampler.cpp @@ -44,7 +44,7 @@ WeightedRealVectorStateSampler::WeightedRealVectorStateSampler(const ompl::base: : ompl::base::StateSampler(space), weights_(weights), bounds_(bounds) { assert(bounds_.rows() == weights_.size()); - if(rng_seed >= 0) + if (rng_seed >= 0) rng_.setLocalSeed(static_cast(rng_seed)); } From a8e2fc51fdbcfbd2d9e9057d6ad0e1d441b82e70 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 12:37:56 -0600 Subject: [PATCH 18/46] Removed OMPL serialization test --- .../test/serialize_test.cpp | 58 ------------------- 1 file changed, 58 deletions(-) diff --git a/tesseract_motion_planners/test/serialize_test.cpp b/tesseract_motion_planners/test/serialize_test.cpp index 7737fa17621..cc340d89cb4 100644 --- a/tesseract_motion_planners/test/serialize_test.cpp +++ b/tesseract_motion_planners/test/serialize_test.cpp @@ -33,10 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include -#include - #include #include #include @@ -76,43 +72,6 @@ TrajOptDefaultPlanProfile getTrajOptPlanProfile() return plan_profile; } -OMPLDefaultPlanProfile getOMPLPlanProfile() -{ - OMPLDefaultPlanProfile ompl_profile; - - ompl_profile.simplify = true; - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - ompl_profile.planners.push_back(std::make_shared()); - - return ompl_profile; -} - DescartesDefaultPlanProfile genDescartesPlanProfile() { DescartesDefaultPlanProfile descartes_profile; @@ -158,23 +117,6 @@ TEST(TesseractMotionPlannersTrajoptSerializeUnit, SerializeTrajoptDefaultPlanToX EXPECT_TRUE(plan_profile.term_type == imported_plan_profile.term_type); } -TEST(TesseractMotionPlannersOMPLSerializeUnit, SerializeOMPLDefaultPlanToXml) // NOLINT -{ - // Write program to file - OMPLDefaultPlanProfile plan_profile = getOMPLPlanProfile(); - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input.xml")); - - // Import file - OMPLDefaultPlanProfile imported_plan_profile = omplPlanFromXMLFile(tesseract_common::getTempPath() + "ompl_default_" - "plan_example_" - "input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "ompl_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.simplify == imported_plan_profile.simplify); -} - TEST(TesseractMotionPlannersDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml) // NOLINT { // Write program to file From 4e68a425dc967822a8712f10947d1139488f0d1a Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 17:11:42 -0600 Subject: [PATCH 19/46] Support state waypoints in OMPL RVSS composite profile --- .../profile/ompl_composite_profile_rvss.cpp | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index d4750ee6982..18d211c1ca4 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -317,6 +317,11 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc const JointWaypoint& jw = start_waypoint.as(); simple_setup->addStartState(createState(jw, env, joint_group, simple_setup->getStateSpace())); } + else if (isStateWaypoint(start_waypoint)) + { + const StateWaypoint& sw = start_waypoint.as(); + simple_setup->addStartState(createState(sw.position, env, joint_group, simple_setup->getStateSpace())); + } else { throw std::runtime_error("Unsupported start waypoint type"); @@ -328,10 +333,12 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc tesseract_common::ManipulatorInfo goal_mi = goal_instruction.getManipulatorInfo().getCombined(instruction.getManipulatorInfo()); + const Waypoint& goal_waypoint = goal_instruction.getWaypoint(); + // Add the goal state - if (isCartesianWaypoint(goal_instruction.getWaypoint())) + if (isCartesianWaypoint(goal_waypoint)) { - const CartesianWaypoint& cw = goal_instruction.getWaypoint().as(); + const CartesianWaypoint& cw = goal_waypoint.as(); tesseract_kinematics::IKSolutions sols = getValidIKSolutions(cw, goal_mi, env); auto states = createOMPLStates(sols, simple_setup->getSpaceInformation()); @@ -342,11 +349,16 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc simple_setup->setGoal(goal_states); } - else if (isJointWaypoint(goal_instruction.getWaypoint())) + else if (isJointWaypoint(goal_waypoint)) { - const JointWaypoint& jw = goal_instruction.getWaypoint().as(); + const JointWaypoint& jw = goal_waypoint.as(); simple_setup->setGoalState(createState(jw, env, joint_group, simple_setup->getStateSpace())); } + else if (isStateWaypoint(goal_waypoint)) + { + const StateWaypoint& sw = goal_waypoint.as(); + simple_setup->addStartState(createState(sw.position, env, joint_group, simple_setup->getStateSpace())); + } else { throw std::runtime_error("Unsupported goal waypoint type"); From a0c589bbdf5dfd73adb09a5331830ccadefabe80 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 17:12:14 -0600 Subject: [PATCH 20/46] Check OMPL planner profile to ensure planner factory vector is not empty --- .../tesseract_motion_planners/ompl/ompl_motion_planner.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 557590d38fc..be537f16a16 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -87,7 +87,12 @@ using OMPLCompositeProfileData = std::tuple { OMPLPlannerParameters params; - inline OMPLPlannerParameters create() const override { return params; }; + inline OMPLPlannerParameters create() const override + { + if (params.planners.empty()) + throw std::runtime_error("No OMPL planner factories defined"); + return params; + }; }; /** From d786538d631507f929f4095fe7128147b31e20d8 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Tue, 23 Nov 2021 17:12:54 -0600 Subject: [PATCH 21/46] Add support for casting request data to simple setup --- .../ompl/src/ompl_motion_planner.cpp | 23 ++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 4fe389a240f..e0fcdfc1266 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -149,6 +149,25 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ return response.status; } + if (request.data != nullptr) + { + ompl::geometric::SimpleSetupPtr ss = std::static_pointer_cast(request.data); + ss->clearStartStates(); + + // Add the start states from the newly configured simple setup to the simple setup from the request + for (unsigned i = 0; i < simple_setup->getProblemDefinition()->getStartStateCount(); ++i) + { + ompl::base::ScopedState<> s(ss->getStateSpace(), simple_setup->getProblemDefinition()->getStartState(i)); + ss->addStartState(s); + } + + // Add the goal states from the newly configured + ss->setGoal(simple_setup->getGoal()); + + // Overwrite the configured simple setup with the modified simple setup from the request + simple_setup.swap(ss); + } + // Create an OMPL planner that can run multiple OMPL planners in parallele auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); @@ -232,10 +251,12 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } else { - // Trajectory length matches seed length, so no need to simplify or interpolate + // Trajectory length matches or exceeds seed length and simplification is disabled, so no need to simplify or + // interpolate } // Get the results + response.data = std::static_pointer_cast(simple_setup); tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); From 1280f1403b11f65c38a79980592cc1e3e3dc4f83 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 24 Nov 2021 10:40:18 -0600 Subject: [PATCH 22/46] Removed unused profile headers from planning server --- .../src/core/process_planning_server.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract_process_managers/src/core/process_planning_server.cpp index 57e96922aec..53b1ff9bbb2 100644 --- a/tesseract_process_managers/src/core/process_planning_server.cpp +++ b/tesseract_process_managers/src/core/process_planning_server.cpp @@ -29,20 +29,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include - -#include - namespace tesseract_planning { ProcessPlanningServer::ProcessPlanningServer(EnvironmentCache::ConstPtr cache, size_t n) From 26d259c30e916f3881f3346944aa0164d2a9e288 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 24 Nov 2021 18:44:29 -0600 Subject: [PATCH 23/46] Fixed interpolation of trajectory according to seed --- .../ompl/src/ompl_motion_planner.cpp | 58 ++++++++++++------- .../test/ompl_planner_tests.cpp | 21 ++++--- 2 files changed, 46 insertions(+), 33 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index e0fcdfc1266..f476d9a7111 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -168,7 +168,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ simple_setup.swap(ss); } - // Create an OMPL planner that can run multiple OMPL planners in parallele + // Create an OMPL planner that can run multiple OMPL planners in parallel auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); // Add all the specified OMPL planners to the parallel planner @@ -234,25 +234,31 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } // The trajectory must have at least as many states as the seed trajectory - const std::size_t n_states = simple_setup->getSolutionPath().getStateCount(); - if (n_states < request.seed.size()) + if (!request.seed.empty()) { - // Upsample the number of states to match the length of the seed - simple_setup->getSolutionPath().interpolate(static_cast(request.seed.size())); - } - else if (n_states > request.seed.size() && params.simplify) - { - // Simplify the solution - simple_setup->simplifySolution(); + const std::size_t n_states = simple_setup->getSolutionPath().getStateCount(); + // The number of states in the seed is the size of the composite instruction plus one for the start state + const unsigned n_seed_states = static_cast(request.seed.at(0).as().size()) + 1; - // Upsample the number of states to match the length of the seed if required - if (simple_setup->getSolutionPath().getStateCount() < request.seed.size()) - simple_setup->getSolutionPath().interpolate(static_cast(request.seed.size())); - } - else - { - // Trajectory length matches or exceeds seed length and simplification is disabled, so no need to simplify or - // interpolate + if (n_states < n_seed_states) + { + // Upsample the number of states to match the length of the seed + simple_setup->getSolutionPath().interpolate(n_seed_states); + } + else if (n_states > n_seed_states && params.simplify) + { + // Simplify the solution + simple_setup->simplifySolution(); + + // Upsample the number of states to match the length of the seed if required + if (simple_setup->getSolutionPath().getStateCount() < n_seed_states) + simple_setup->getSolutionPath().interpolate(n_seed_states); + } + else + { + // Trajectory length matches or exceeds seed length and simplification is disabled, so no need to simplify or + // interpolate + } } // Get the results @@ -282,9 +288,11 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ // The results composite will only have as many states as the seed, but the OMPL trajectory might require more states // In this case, we need to insert more states into the composite to cover the difference - if (composite.size() < trajectory.rows()) + // Remember the composite does not include the start state, so compare its size with one less than the size of the + // trajectory + if (composite.size() < trajectory.rows() - 1) { - const std::size_t diff = static_cast(trajectory.rows()) - composite.size(); + const std::size_t diff = static_cast(trajectory.rows() - 1) - composite.size(); composite.insert(composite.end(), diff, composite.back()); } @@ -299,7 +307,10 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ else { // Subsequent trajectory states go into the composite instruction - auto& move_instruction = composite.at(static_cast(i)).as(); + // The index into the composite of these states is one less than the index of the trajectory state since the first + // trajectory state was saved outside the composite + const std::size_t composite_idx = static_cast(i - 1); + auto& move_instruction = composite.at(composite_idx).as(); move_instruction.getWaypoint().as().position = trajectory.row(i); } } @@ -320,7 +331,10 @@ bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) std::runtime_error("Request contains no instructions"); if (request.instructions.size() > 1) - std::runtime_error("OMPL planner only supports one plan instruction"); + std::runtime_error("OMPL planner instruction only supports one plan instruction"); + + if (request.seed.size() > 1) + std::runtime_error("OMPL planner seed only supports a single trajectory"); // Attempt to cast the first child instruction to a PlanInstruction request.instructions.at(0).as(); diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 5dcb22b2d40..0dc728c0c75 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -27,7 +27,6 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include - #include #include #include @@ -166,8 +165,8 @@ class OMPLTestFixture : public ::testing::Test const std::vector end_state_ = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; /** @brief Motion planning profile name */ const std::string profile_name_ = "TEST_PROFILE"; - /** @brief Number of states in the seed trajectory */ - const int seed_size_ = 10; + /** @brief Number of steps in the seed trajectory, equivalent to the the number of states minus one */ + const int seed_steps_ = 10; }; using Implementations = ::testing::Typesenv, 3.14, 1.0, 3.14, this->seed_size_); + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.env = this->env; request.env_state = cur_state; @@ -241,7 +240,7 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT ASSERT_TRUE(&status); EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); EXPECT_EQ(planner_response.results.size(), 1); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); EXPECT_TRUE(wp2.isApprox( @@ -277,7 +276,7 @@ TYPED_TEST(OMPLTestFixture, StartStateInCollision) // Update Configuration PlannerRequest request; request.instructions = program; - request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.env = this->env; request.env_state = cur_state; @@ -322,7 +321,7 @@ TYPED_TEST(OMPLTestFixture, EndStateInCollision) program.push_back(plan_f1); PlannerRequest request; - request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.instructions = program; request.env = this->env; request.env_state = cur_state; @@ -375,7 +374,7 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.env = this->env; request.env_state = cur_state; @@ -398,7 +397,7 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) } ASSERT_TRUE(&status); ASSERT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); const MoveInstruction* last_move = getLastMoveInstruction(planner_response.results); @@ -439,7 +438,7 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) // Create Planner Request PlannerRequest request; request.instructions = program; - request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_size_); + request.seed = generateSeed(program, cur_state, this->env, 3.14, 1.0, 3.14, this->seed_steps_); request.env = this->env; request.env_state = cur_state; @@ -463,7 +462,7 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) ASSERT_TRUE(&status); ASSERT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), request.seed.size()); + EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); const MoveInstruction* last_mi = getLastMoveInstruction(planner_response.results); EXPECT_TRUE(wp2.isApprox(getJointPosition(last_mi->getWaypoint()), 1e-5)); From b3614cb26ca542df4cdd14aad10ce473038be565 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 24 Nov 2021 18:44:49 -0600 Subject: [PATCH 24/46] Saved OMPL planner data as the response data instead of the simple setup; added skeleton for loading a planner with pre-populated data --- .../ompl/src/ompl_motion_planner.cpp | 63 ++++++++++++------- 1 file changed, 42 insertions(+), 21 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index f476d9a7111..aa440e6de4e 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -149,31 +149,36 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ return response.status; } - if (request.data != nullptr) - { - ompl::geometric::SimpleSetupPtr ss = std::static_pointer_cast(request.data); - ss->clearStartStates(); - - // Add the start states from the newly configured simple setup to the simple setup from the request - for (unsigned i = 0; i < simple_setup->getProblemDefinition()->getStartStateCount(); ++i) - { - ompl::base::ScopedState<> s(ss->getStateSpace(), simple_setup->getProblemDefinition()->getStartState(i)); - ss->addStartState(s); - } - - // Add the goal states from the newly configured - ss->setGoal(simple_setup->getGoal()); - - // Overwrite the configured simple setup with the modified simple setup from the request - simple_setup.swap(ss); - } - // Create an OMPL planner that can run multiple OMPL planners in parallel auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); // Add all the specified OMPL planners to the parallel planner + std::vector planners; + planners.reserve(params.planners.size()); for (const auto& factory : params.planners) - parallel_plan->addPlanner(factory->create(simple_setup->getSpaceInformation())); + { + ompl::base::PlannerPtr planner = factory->create(simple_setup->getSpaceInformation()); + + // // TODO: construct planner from the saved planner data (only valid for multi-query planners, such as PRM) + // if (request.data != nullptr) + // { + // auto planner_data = std::static_pointer_cast(request.data); + // planner = factory->create(*planner_data); + // if (!planner) + // { + // CONSOLE_BRIDGE_logWarn("Unable to construct planner with saved PlannerData; constructing planner from + // scratch instead"); planner = factory->create(simple_setup->getSpaceInformation()); + // } + // } + // else + // { + // planner = factory->create(simple_setup->getSpaceInformation()); + // } + + // Add the planner to the parallel planner object and save a pointer to it + parallel_plan->addPlanner(planner); + planners.push_back(planner); + } ompl::base::PlannerStatus status; if (!params.optimize) @@ -226,6 +231,23 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } } + // Extract the planner data from each planner and concatenate + // TODO: Investigate whether it is problematic to concatenate planner data from multiple different planners. The + // getPlannerData function handles new vertex and edge indices correctly, but vertices that are close enough in the + // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways + // and set different vertex/edge properties, so it may not make sense to combine them + auto planner_data = std::make_shared(simple_setup->getSpaceInformation()); + for (auto planner : planners) + planner->getPlannerData(*planner_data); + + // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by + // decoupling from the planner instances so the data is not lost when the planner instances go out of scope + planner_data->decoupleFromPlanner(); + + // Save the combined planner data in the response + response.data = std::static_pointer_cast(planner_data); + + // Check the planner status if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) { response.status = @@ -262,7 +284,6 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } // Get the results - response.data = std::static_pointer_cast(simple_setup); tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); From 995d6c80156dd5585b4c1626aac76295a63a9ad0 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 8 Dec 2021 22:46:53 -0600 Subject: [PATCH 25/46] Separate start/end states in OMPL unit test to prevent collisions with separating wall during sampling --- tesseract_motion_planners/test/ompl_planner_tests.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 0dc728c0c75..e0bfe889007 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -160,9 +160,9 @@ class OMPLTestFixture : public ::testing::Test /** @brief Tesseract OMPL motion planner */ const OMPLMotionPlanner ompl_planner; /** @brief Arbitrary trajectory start state */ - const std::vector start_state_ = { -0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + const std::vector start_state_ = { -0.6, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; /** @brief Arbitrary trajectory end state */ - const std::vector end_state_ = { 0.5, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; + const std::vector end_state_ = { 0.6, 0.5, 0.0, -1.3348, 0.0, 1.4959, 0.0 }; /** @brief Motion planning profile name */ const std::string profile_name_ = "TEST_PROFILE"; /** @brief Number of steps in the seed trajectory, equivalent to the the number of states minus one */ From db4c8e14a8a74069e8332ddb7075d6c60a788244 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 9 Dec 2021 00:46:59 -0600 Subject: [PATCH 26/46] Implemented first pass at waypoint profile --- tesseract_motion_planners/ompl/CMakeLists.txt | 3 +- .../ompl/ompl_motion_planner.h | 1 + .../ompl/profile/ompl_waypoint_profile.h | 16 + .../ompl/src/ompl_motion_planner.cpp | 312 +++++++++++------- .../profile/ompl_composite_profile_rvss.cpp | 240 +------------- .../src/profile/ompl_waypoint_profile.cpp | 128 +++++++ 6 files changed, 350 insertions(+), 350 deletions(-) create mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h create mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index cf761e51475..81e011d48e1 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -16,7 +16,8 @@ set(OMPL_SRC src/ompl_motion_planner.cpp src/utils.cpp # Profiles - src/profile/ompl_composite_profile_rvss.cpp) + src/profile/ompl_composite_profile_rvss.cpp + src/profile/ompl_waypoint_profile.cpp) # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif() diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index be537f16a16..b0343bcf90e 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include #include #include diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h new file mode 100644 index 00000000000..345da98e185 --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h @@ -0,0 +1,16 @@ +#ifndef TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H +#define TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H + +#include + +namespace tesseract_planning +{ +struct OMPLWaypointProfile : public WaypointProfile> +{ + virtual std::vector create(const Instruction& instruction, + tesseract_environment::Environment::ConstPtr env) const override; +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index aa440e6de4e..776bd5cb88e 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -80,75 +80,28 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, return false; } -/** @brief Construct a basic planner */ -OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : name_(std::move(name)) -{ - if (name_.empty()) - throw std::runtime_error("OMPLMotionPlanner name is empty!"); -} - -const std::string& OMPLMotionPlanner::getName() const { return name_; } - -bool OMPLMotionPlanner::terminate() +std::vector> createOMPLStates(const std::vector& joint_states, + ompl::base::SpaceInformationPtr si) { - CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet"); - return false; -} + std::vector> states; + states.reserve(joint_states.size()); -void OMPLMotionPlanner::clear() {} - -tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, - PlannerResponse& response, - bool verbose) const -{ - auto status_category_ = std::make_shared(name_); - - // Check the format of the request - if (!checkUserInput(request)) // NOLINT + for (const auto& js : joint_states) { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } + ompl::base::ScopedState<> state(si->getStateSpace()); + for (unsigned i = 0; i < js.size(); ++i) + state[i] = js[static_cast(i)]; - // If the verbose set the log level to debug. - if (verbose) - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - - // Get the planner profile - OMPLPlannerParameters params; - try - { - const std::string profile_name = - getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); - auto pp = request.profiles->getProfile>(name_, profile_name); - params = pp->create(); - } - catch (const std::exception& ex) - { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; + states.push_back(state); } - // Get the composite profile - ompl::geometric::SimpleSetupPtr simple_setup; - OMPLStateExtractor extractor; - try - { - const std::string profile_name = - getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - auto cp = request.profiles->getProfile>(name_, profile_name); - std::tie(simple_setup, extractor) = cp->create(request.instructions, request.env); - } - catch (const std::exception& ex) - { - CONSOLE_BRIDGE_logError(ex.what()); - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } + return states; +} +std::shared_ptr plan(ompl::geometric::SimpleSetupPtr simple_setup, + const OMPLPlannerParameters& params, + const unsigned n_output_states) +{ // Create an OMPL planner that can run multiple OMPL planners in parallel auto parallel_plan = std::make_shared(simple_setup->getProblemDefinition()); @@ -231,50 +184,28 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } } - // Extract the planner data from each planner and concatenate - // TODO: Investigate whether it is problematic to concatenate planner data from multiple different planners. The - // getPlannerData function handles new vertex and edge indices correctly, but vertices that are close enough in the - // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways - // and set different vertex/edge properties, so it may not make sense to combine them - auto planner_data = std::make_shared(simple_setup->getSpaceInformation()); - for (auto planner : planners) - planner->getPlannerData(*planner_data); - - // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by - // decoupling from the planner instances so the data is not lost when the planner instances go out of scope - planner_data->decoupleFromPlanner(); - - // Save the combined planner data in the response - response.data = std::static_pointer_cast(planner_data); - // Check the planner status if (status != ompl::base::PlannerStatus::EXACT_SOLUTION) - { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, status_category_); - return response.status; - } + throw std::runtime_error("Planning failed to find exact solution"); // The trajectory must have at least as many states as the seed trajectory - if (!request.seed.empty()) + if (n_output_states > 2) { const std::size_t n_states = simple_setup->getSolutionPath().getStateCount(); - // The number of states in the seed is the size of the composite instruction plus one for the start state - const unsigned n_seed_states = static_cast(request.seed.at(0).as().size()) + 1; - if (n_states < n_seed_states) + if (n_states < n_output_states) { // Upsample the number of states to match the length of the seed - simple_setup->getSolutionPath().interpolate(n_seed_states); + simple_setup->getSolutionPath().interpolate(n_output_states); } - else if (n_states > n_seed_states && params.simplify) + else if (n_states > n_output_states && params.simplify) { // Simplify the solution simple_setup->simplifySolution(); // Upsample the number of states to match the length of the seed if required - if (simple_setup->getSolutionPath().getStateCount() < n_seed_states) - simple_setup->getSolutionPath().interpolate(n_seed_states); + if (simple_setup->getSolutionPath().getStateCount() < n_output_states) + simple_setup->getSolutionPath().interpolate(n_output_states); } else { @@ -283,29 +214,36 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } } - // Get the results - tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); - assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); - assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); + // Extract the planner data from each planner and concatenate + // TODO: Investigate whether it is problematic to concatenate planner data from multiple different planners. The + // getPlannerData function handles new vertex and edge indices correctly, but vertices that are close enough in the + // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways + // and set different vertex/edge properties, so it may not make sense to combine them + auto planner_data = std::make_shared(simple_setup->getSpaceInformation()); + for (auto planner : planners) + planner->getPlannerData(*planner_data); - // Enforce limits - { - const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; - for (Eigen::Index i = 0; i < trajectory.rows(); i++) - tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); - } + // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by + // decoupling from the planner instances so the data is not lost when the planner instances go out of scope + planner_data->decoupleFromPlanner(); + return planner_data; +} + +CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArray& trajectory, + const CompositeInstruction& input_instructions, + const CompositeInstruction& seed) +{ // Initialize the output by copying the input program and flattening it - response.results = request.seed; - std::vector> results_flattened = - flattenProgramToPattern(response.results, request.instructions); + CompositeInstruction output(seed); + std::vector> output_flattened = + flattenProgramToPattern(output, input_instructions); // Element 0 of the flattened results is a start instruction that should contain the first trajectory state - auto& start_instruction = results_flattened.at(0).get().as(); + auto& start_instruction = output_flattened.at(0).get().as(); // Element 1 of the flattened results is a composite instruction containing the reset of the trajectory - auto& composite = results_flattened.at(1).get().as(); + auto& composite = output_flattened.at(1).get().as(); // The results composite will only have as many states as the seed, but the OMPL trajectory might require more states // In this case, we need to insert more states into the composite to cover the difference @@ -336,6 +274,155 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } } + return output; +} + +/** @brief Construct a basic planner */ +OMPLMotionPlanner::OMPLMotionPlanner(std::string name) : name_(std::move(name)) +{ + if (name_.empty()) + throw std::runtime_error("OMPLMotionPlanner name is empty!"); +} + +const std::string& OMPLMotionPlanner::getName() const { return name_; } + +bool OMPLMotionPlanner::terminate() +{ + CONSOLE_BRIDGE_logWarn("Termination of ongoing optimization is not implemented yet"); + return false; +} + +void OMPLMotionPlanner::clear() {} + +tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& request, + PlannerResponse& response, + bool verbose) const +{ + auto status_category_ = std::make_shared(name_); + + // Check the format of the request + if (!checkUserInput(request)) // NOLINT + { + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + + // If the verbose set the log level to debug. + if (verbose) + console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); + + // Get the planner profile + OMPLPlannerParameters params; + try + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); + auto pp = request.profiles->getProfile>(name_, profile_name); + params = pp->create(); + } + catch (const std::exception&) + { + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + + // Get the composite profile + ompl::geometric::SimpleSetupPtr simple_setup; + OMPLStateExtractor extractor; + try + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + auto cp = request.profiles->getProfile>(name_, profile_name); + std::tie(simple_setup, extractor) = cp->create(request.instructions, request.env); + } + catch (const std::exception& ex) + { + CONSOLE_BRIDGE_logError(ex.what()); + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); + return response.status; + } + + // Loop over each simple setup produced by the composite waypoint + for (std::size_t i = 0; i < 1; ++i) + { + simple_setup->clearStartStates(); + + // Get the start waypoint profile and add the states to the SimpleSetup + if (i == 0) + { + const PlanInstruction& pi = request.instructions[i].as(); + + const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + auto p = request.profiles->getProfile>>(name_, profile_name); + + auto states = createOMPLStates(p->create(pi, request.env), simple_setup->getSpaceInformation()); + + // Add the states to the SimpleSetup + std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { + simple_setup->addStartState(state); + }); + } + else + { + // Use the last state of the previous trajectory as the single start state for this plan + + } + + // Get the end waypoint profile and add the states to the simple setup + { + const PlanInstruction& pi = request.instructions[i + 1].as(); + + const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + auto p = request.profiles->getProfile>>(name_, profile_name); + + auto states = createOMPLStates(p->create(pi, request.env), simple_setup->getSpaceInformation()); + + auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); + std::for_each(states.begin(), states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { + goal_states->addState(state); + }); + + simple_setup->setGoal(goal_states); + } + + // The number of states in the seed is the size of the composite instruction plus one for the start state + const unsigned n_seed_states = static_cast(request.seed.at(i).as().size()) + 1; + + // Plan + auto planner_data = plan(simple_setup, params, n_seed_states); + + // Get the results + tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); + assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); + assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); + + // Enforce limits + { + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; + for (Eigen::Index i = 0; i < trajectory.rows(); i++) + tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); + } + + // Construct the output + CompositeInstruction foo = buildTrajectoryInstruction(trajectory, request.instructions, request.seed[i].as()); + } + + + + // Save the combined planner data in the response +// response.data = std::static_pointer_cast(planner_data); + + + + + + + response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); return response.status; } @@ -351,14 +438,9 @@ bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) if (request.instructions.empty()) std::runtime_error("Request contains no instructions"); - if (request.instructions.size() > 1) - std::runtime_error("OMPL planner instruction only supports one plan instruction"); - - if (request.seed.size() > 1) - std::runtime_error("OMPL planner seed only supports a single trajectory"); - - // Attempt to cast the first child instruction to a PlanInstruction - request.instructions.at(0).as(); + if (request.instructions.size() != request.seed.size()) + std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) + + ") does not match seed size (" + std::to_string(request.seed.size()) + ")"); return true; } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index 18d211c1ca4..88c3e413db3 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -16,161 +16,15 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -tesseract_collision::ContactResultMap checkCollision(const Eigen::VectorXd& state, - tesseract_kinematics::JointGroup::ConstPtr manip, - tesseract_collision::DiscreteContactManager::Ptr contact_checker) -{ - tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); - - for (const auto& link_name : contact_checker->getActiveCollisionObjects()) - contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); - - tesseract_collision::ContactResultMap contact_map; - contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); - - return contact_map; -} - -std::vector> createOMPLStates(const tesseract_kinematics::IKSolutions& joint_states, - ompl::base::SpaceInformationPtr si) -{ - std::vector> states; - states.reserve(joint_states.size()); - - for (const auto& js : joint_states) - { - ompl::base::ScopedState<> state(si->getStateSpace()); - for (unsigned i = 0; i < js.size(); ++i) - state[i] = js[static_cast(i)]; - - states.push_back(state); - } - - return states; -} - -tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& cartesian_waypoint, - const ManipulatorInfo& mi, - tesseract_environment::Environment::ConstPtr env) -{ - if (mi.manipulator.empty()) - throw std::runtime_error("OMPL: manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("OMPL: tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("OMPL: working_frame is empty!"); - - // Get the kinematics group for solving IK - tesseract_kinematics::KinematicGroup::ConstPtr manip = - env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); - - /** Solve IK - * @todo Need to add Descartes pose sample to ompl profile - */ - Eigen::Isometry3d tcp_offset = env->findTCPOffset(mi); - Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); - tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); - const tesseract_kinematics::IKSolutions joint_solutions = - manip->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(manip->numJoints())); - - // Check collision - std::vector contact_map_vec(static_cast(joint_solutions.size())); - tesseract_common::KinematicLimits limits = manip->getLimits(); - tesseract_kinematics::IKSolutions valid_solutions; - valid_solutions.reserve(joint_solutions.size()); - for (std::size_t i = 0; i < joint_solutions.size(); ++i) - { - Eigen::VectorXd solution = joint_solutions[i]; - - // Check limits - if (tesseract_common::satisfiesPositionLimits(solution, limits.joint_limits)) - { - tesseract_common::enforcePositionLimits(solution, limits.joint_limits); - } - else - { - CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); - continue; - } - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - contact_map_vec[i] = checkCollision(solution, manip, env->getDiscreteContactManager()); - if (contact_map_vec[i].empty()) - { - valid_solutions.push_back(solution); - - auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( - solution, limits.joint_limits, manip->getRedundancyCapableJointIndices()); - - valid_solutions.insert(valid_solutions.end(), redundant_solutions.begin(), redundant_solutions.end()); - } - } - - if (valid_solutions.empty()) - { - for (std::size_t i = 0; i < contact_map_vec.size(); i++) - for (const auto& contact_vec : contact_map_vec[i]) - for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Solution: " + std::to_string(i) + " Links: " + contact.link_names[0] + ", " + - contact.link_names[1] + " Distance: " + std::to_string(contact.distance)) - .c_str()); - throw std::runtime_error("In OMPLDefaultPlanProfile: All goal states are either in collision or outside limits"); - } - - return valid_solutions; -} - -ompl::base::ScopedState<> createState(Eigen::VectorXd joint_waypoint, - tesseract_environment::Environment::ConstPtr env, - tesseract_kinematics::JointGroup::ConstPtr manip, - ompl::base::StateSpacePtr ss) -{ - const auto dof = manip->numJoints(); - tesseract_common::KinematicLimits limits = manip->getLimits(); - - if (tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) - tesseract_common::enforcePositionLimits(joint_waypoint, limits.joint_limits); - else - throw std::runtime_error("Start state is outside limits"); - - // Get discrete contact manager for testing provided start and end position - // This is required because collision checking happens in motion validators now - // instead of the isValid function to avoid unnecessary collision checks. - tesseract_collision::ContactResultMap contact_map = - checkCollision(joint_waypoint, manip, env->getDiscreteContactManager()); - if (!contact_map.empty()) - { - std::stringstream ss; - ss << "Start state is in collision"; - for (const auto& contact_vec : contact_map) - for (const auto& contact : contact_vec.second) - ss << "\n\tLinks: " << contact.link_names[0] << ", " << contact.link_names[1] - << " Distance: " << contact.distance; - throw std::runtime_error(ss.str()); - } - - ompl::base::ScopedState<> start_state(ss); - for (unsigned i = 0; i < dof; ++i) - start_state[i] = joint_waypoint[i]; - - return start_state; -} - OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruction& instruction, tesseract_environment::Environment::ConstPtr env) const { - if (instruction.size() > 1) - throw std::runtime_error("The composite instruction contains more than one instruction. The composite instruction " - "can " - "only contain one child and it must be a PlanInstruction type"); - - const PlanInstruction& goal_instruction = instruction.at(0).as(); - if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) - throw std::runtime_error("Move types other than freespace are not currently supported"); + for (const Instruction& inst : instruction) + { + const PlanInstruction& goal_instruction = inst.as(); + if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) + throw std::runtime_error("Only freespace plan instruction types are currently supported"); + } // Get the joint group information from the composite instruction and environment const tesseract_kinematics::JointGroup::ConstPtr joint_group = @@ -283,88 +137,6 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc optimization_objective_allocator(simple_setup->getSpaceInformation())); } - // Add the start waypoint - { - // Get the start waypoint - Waypoint start_waypoint{ NullWaypoint() }; - tesseract_common::ManipulatorInfo start_mi = instruction.getManipulatorInfo(); - try - { - const auto& pi = instruction.getStartInstruction().as(); - start_waypoint = pi.getWaypoint(); - start_mi = start_mi.getCombined(pi.getManipulatorInfo()); - } - catch (const std::bad_cast& ex) - { - Eigen::VectorXd current_jv = env->getCurrentJointValues(); - start_waypoint = StateWaypoint(joint_names, current_jv); - } - - // Add the start state(s) - if (isCartesianWaypoint(start_waypoint)) - { - const CartesianWaypoint& cw = start_waypoint.as(); - tesseract_kinematics::IKSolutions sols = getValidIKSolutions(cw, start_mi, env); - auto states = createOMPLStates(sols, simple_setup->getSpaceInformation()); - - // Add the states to the SimpleSetup - std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { - simple_setup->addStartState(state); - }); - } - else if (isJointWaypoint(start_waypoint)) - { - const JointWaypoint& jw = start_waypoint.as(); - simple_setup->addStartState(createState(jw, env, joint_group, simple_setup->getStateSpace())); - } - else if (isStateWaypoint(start_waypoint)) - { - const StateWaypoint& sw = start_waypoint.as(); - simple_setup->addStartState(createState(sw.position, env, joint_group, simple_setup->getStateSpace())); - } - else - { - throw std::runtime_error("Unsupported start waypoint type"); - } - } - - // Add the goal waypoint - { - tesseract_common::ManipulatorInfo goal_mi = - goal_instruction.getManipulatorInfo().getCombined(instruction.getManipulatorInfo()); - - const Waypoint& goal_waypoint = goal_instruction.getWaypoint(); - - // Add the goal state - if (isCartesianWaypoint(goal_waypoint)) - { - const CartesianWaypoint& cw = goal_waypoint.as(); - tesseract_kinematics::IKSolutions sols = getValidIKSolutions(cw, goal_mi, env); - auto states = createOMPLStates(sols, simple_setup->getSpaceInformation()); - - auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); - std::for_each(states.begin(), states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { - goal_states->addState(state); - }); - - simple_setup->setGoal(goal_states); - } - else if (isJointWaypoint(goal_waypoint)) - { - const JointWaypoint& jw = goal_waypoint.as(); - simple_setup->setGoalState(createState(jw, env, joint_group, simple_setup->getStateSpace())); - } - else if (isStateWaypoint(goal_waypoint)) - { - const StateWaypoint& sw = goal_waypoint.as(); - simple_setup->addStartState(createState(sw.position, env, joint_group, simple_setup->getStateSpace())); - } - else - { - throw std::runtime_error("Unsupported goal waypoint type"); - } - } - // Return the composite data return std::make_tuple(simple_setup, extractor); } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp new file mode 100644 index 00000000000..278bd73baad --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -0,0 +1,128 @@ +#include +#include + +namespace tesseract_planning +{ +void checkCollision(const Eigen::VectorXd& state, + tesseract_environment::Environment::ConstPtr env, + tesseract_kinematics::JointGroup::ConstPtr manip) +{ + tesseract_collision::DiscreteContactManager::Ptr contact_checker = env->getDiscreteContactManager(); + tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); + + for (const auto& link_name : contact_checker->getActiveCollisionObjects()) + contact_checker->setCollisionObjectsTransform(link_name, link_transforms[link_name]); + + tesseract_collision::ContactResultMap contact_map; + contact_checker->contactTest(contact_map, tesseract_collision::ContactTestType::FIRST); + + if (!contact_map.empty()) + { + std::stringstream ss; + ss << "State is in collision"; + for (const auto& contact_vec : contact_map) + for (const auto& contact : contact_vec.second) + ss << "\n\tLinks: " << contact.link_names[0] << ", " << contact.link_names[1] + << " Distance: " << contact.distance; + throw std::runtime_error(ss.str()); + } +} + +Eigen::VectorXd updateLimits(Eigen::Ref joint_waypoint, tesseract_common::KinematicLimits limits) +{ + if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) + throw std::runtime_error("State violates joint limits"); + + Eigen::VectorXd tmp(joint_waypoint); + tesseract_common::enforcePositionLimits(tmp, limits.joint_limits); + return tmp; +} + +tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& cartesian_waypoint, + const ManipulatorInfo& mi, + tesseract_environment::Environment::ConstPtr env) +{ + if (mi.manipulator.empty()) + throw std::runtime_error("OMPL: manipulator is empty!"); + + if (mi.tcp_frame.empty()) + throw std::runtime_error("OMPL: tcp_frame is empty!"); + + if (mi.working_frame.empty()) + throw std::runtime_error("OMPL: working_frame is empty!"); + + // Get the kinematics group for solving IK + tesseract_kinematics::KinematicGroup::ConstPtr manip = + env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); + + Eigen::Isometry3d tcp_offset = env->findTCPOffset(mi); + Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); + tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); + const tesseract_kinematics::IKSolutions joint_solutions = + manip->calcInvKin({ ik_input }, Eigen::VectorXd::Zero(manip->numJoints())); + + // Assemble all the valid solutionss + tesseract_common::KinematicLimits limits = manip->getLimits(); + tesseract_kinematics::IKSolutions valid_solutions; + valid_solutions.reserve(joint_solutions.size()); + for (std::size_t i = 0; i < joint_solutions.size(); ++i) + { + try + { + // Update the joint solution based on the kinematic limits + Eigen::VectorXd solution = updateLimits(joint_solutions[i], limits); + + // Ensure the solution is collision-free + checkCollision(solution, env, manip); + + // Add the solution to the container + valid_solutions.push_back(solution); + + // Add the redundant solutions + auto redundant_solutions = tesseract_kinematics::getRedundantSolutions( + solution, limits.joint_limits, manip->getRedundancyCapableJointIndices()); + valid_solutions.insert(valid_solutions.end(), redundant_solutions.begin(), redundant_solutions.end()); + } + catch(const std::exception& ex) + { + CONSOLE_BRIDGE_logDebug(ex.what()); + continue; + } + } + + if (valid_solutions.empty()) + throw std::runtime_error("All states are either in collision or outside limits"); + + return valid_solutions; +} + +std::vector OMPLWaypointProfile::create(const Instruction& instruction, + tesseract_environment::Environment::ConstPtr env) const +{ + PlanInstruction plan_instruction = instruction.as(); + tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo(); + const Waypoint& waypoint = plan_instruction.getWaypoint(); + + if (isCartesianWaypoint(waypoint)) + { + const CartesianWaypoint& cw = waypoint.as(); + return getValidIKSolutions(cw, mi, env); + } + else if (isJointWaypoint(waypoint)) + { + const JointWaypoint& jw = waypoint.as(); + const Eigen::VectorXd updated_state = updateLimits(jw, env->getJointGroup(mi.manipulator)->getLimits()); + return { updated_state }; + } + else if (isStateWaypoint(waypoint)) + { + const StateWaypoint& sw = waypoint.as(); + Eigen::Map state(sw.position.data(), sw.position.size()); + const Eigen::VectorXd updated_state = updateLimits(state, env->getJointGroup(mi.manipulator)->getLimits()); + return { updated_state }; + } + + throw std::runtime_error("Unsupported waypoint type"); +} + +} // namespace tesseract_planning From 659035cebb037dda1821c45a73ae3cc1f5291041 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 13:55:50 -0600 Subject: [PATCH 27/46] Added waypoint profile to unit test --- .../test/ompl_planner_tests.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index e0bfe889007..37e03bca16b 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -201,10 +202,10 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -227,6 +228,8 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); d->addProfile>( OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + d->addProfile>>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); request.profiles = std::move(d); } @@ -258,13 +261,13 @@ TYPED_TEST(OMPLTestFixture, StartStateInCollision) const std::vector swp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; const JointWaypoint wp1(joint_group->getJointNames(), Eigen::Map(swp.data(), static_cast(swp.size()))); - const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Specify a end waypoint const JointWaypoint wp2( joint_group->getJointNames(), Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); - const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a new program CompositeInstruction program; @@ -287,6 +290,8 @@ TYPED_TEST(OMPLTestFixture, StartStateInCollision) OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); d->addProfile>( OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + d->addProfile>>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); request.profiles = std::move(d); } @@ -305,13 +310,13 @@ TYPED_TEST(OMPLTestFixture, EndStateInCollision) const JointWaypoint wp1( joint_group->getJointNames(), Eigen::Map(this->start_state_.data(), static_cast(this->start_state_.size()))); - const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Check for end state in collision error const std::vector ewp = { 0, 0.7, 0.0, 0, 0.0, 0, 0.0 }; const JointWaypoint wp2(joint_group->getJointNames(), Eigen::Map(ewp.data(), static_cast(ewp.size()))); - const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a new program CompositeInstruction program; @@ -333,6 +338,8 @@ TYPED_TEST(OMPLTestFixture, EndStateInCollision) OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); d->addProfile>( OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + d->addProfile>>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); request.profiles = std::move(d); } @@ -359,10 +366,10 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) const CartesianWaypoint wp2 = goal; // Define Start Instruction - const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -385,6 +392,8 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); d->addProfile>( OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + d->addProfile>>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); request.profiles = std::move(d); } @@ -423,10 +432,10 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) Eigen::Map(this->end_state_.data(), static_cast(this->end_state_.size()))); // Define Start Instruction - const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_); + const PlanInstruction start_instruction(wp1, PlanInstructionType::START, this->profile_name_, this->manip); // Define Plan Instructions - const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_); + const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -449,6 +458,8 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); d->addProfile>( OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); + d->addProfile>>( + OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); request.profiles = std::move(d); } From 7e47aca733fd4940e9fc17de381128c452edbe5b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 13:56:19 -0600 Subject: [PATCH 28/46] Fixed creation of trajectory composite instruction --- .../ompl/src/ompl_motion_planner.cpp | 73 +++++++++---------- 1 file changed, 34 insertions(+), 39 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 776bd5cb88e..5134d3afb1f 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -231,28 +231,19 @@ std::shared_ptr plan(ompl::geometric::SimpleSetupPtr si } CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArray& trajectory, - const CompositeInstruction& input_instructions, const CompositeInstruction& seed) { // Initialize the output by copying the input program and flattening it CompositeInstruction output(seed); - std::vector> output_flattened = - flattenProgramToPattern(output, input_instructions); - - // Element 0 of the flattened results is a start instruction that should contain the first trajectory state - auto& start_instruction = output_flattened.at(0).get().as(); - - // Element 1 of the flattened results is a composite instruction containing the reset of the trajectory - auto& composite = output_flattened.at(1).get().as(); // The results composite will only have as many states as the seed, but the OMPL trajectory might require more states // In this case, we need to insert more states into the composite to cover the difference // Remember the composite does not include the start state, so compare its size with one less than the size of the // trajectory - if (composite.size() < trajectory.rows() - 1) + if (seed.size() < trajectory.rows() - 1) { - const std::size_t diff = static_cast(trajectory.rows() - 1) - composite.size(); - composite.insert(composite.end(), diff, composite.back()); + const std::size_t diff = static_cast(trajectory.rows() - 1) - output.size(); + output.insert(output.end(), diff, output.back()); } // Overwrite the contents of each copied waypoint @@ -261,7 +252,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra // The first trajectory state goes into the start instruction if (i == 0) { - start_instruction.getWaypoint().as().position = trajectory.row(i); + output.getStartInstruction().as().getWaypoint().as().position = trajectory.row(i); } else { @@ -269,7 +260,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra // The index into the composite of these states is one less than the index of the trajectory state since the first // trajectory state was saved outside the composite const std::size_t composite_idx = static_cast(i - 1); - auto& move_instruction = composite.at(composite_idx).as(); + auto& move_instruction = output.at(composite_idx).as(); move_instruction.getWaypoint().as().position = trajectory.row(i); } } @@ -346,35 +337,44 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ return response.status; } - // Loop over each simple setup produced by the composite waypoint - for (std::size_t i = 0; i < 1; ++i) + // Set up the output trajectory to be a composite of composites + response.results.reserve(request.instructions.size()); + + // Loop over each pair of waypoints + for (std::size_t i = 0; i < request.instructions.size(); ++i) { simple_setup->clearStartStates(); - // Get the start waypoint profile and add the states to the SimpleSetup - if (i == 0) + // Add the start state(s) { - const PlanInstruction& pi = request.instructions[i].as(); + std::vector start_states; - const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - auto p = request.profiles->getProfile>>(name_, profile_name); - - auto states = createOMPLStates(p->create(pi, request.env), simple_setup->getSpaceInformation()); + // Get the start waypoint profile and add the states to the SimpleSetup + if (i == 0) + { + const PlanInstruction& pi = request.instructions.getStartInstruction().as(); + const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + auto p = request.profiles->getProfile>>(name_, profile_name); + start_states = p->create(pi, request.env); + } + else + { + // Use the last state of the previous trajectory as the single start state for this plan + const MoveInstruction& mi = response.results.back().as().back().as(); + const StateWaypoint& sw = mi.getWaypoint().as(); + start_states.push_back(sw.position); + } // Add the states to the SimpleSetup + auto states = createOMPLStates(start_states, simple_setup->getSpaceInformation()); std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { simple_setup->addStartState(state); }); } - else - { - // Use the last state of the previous trajectory as the single start state for this plan - - } - // Get the end waypoint profile and add the states to the simple setup + // Add the goal waypoint(s) { - const PlanInstruction& pi = request.instructions[i + 1].as(); + const PlanInstruction& pi = request.instructions[i].as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); auto p = request.profiles->getProfile>>(name_, profile_name); @@ -408,21 +408,16 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); } - // Construct the output - CompositeInstruction foo = buildTrajectoryInstruction(trajectory, request.instructions, request.seed[i].as()); + // Construct the output trajectory instruction and add it to the response + response.results.push_back(buildTrajectoryInstruction(trajectory, request.seed[i].as())); } - + // Set top-level composite start instruction to first waypoint of first trajectory + response.results.getStartInstruction() = response.results.at(0).as().getStartInstruction(); // Save the combined planner data in the response // response.data = std::static_pointer_cast(planner_data); - - - - - - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); return response.status; } From f6fc3bdc0609e37f4eb4a95307fb373ee384a8ca Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 15:14:08 -0600 Subject: [PATCH 29/46] Updated OMPL planner profiles to match new interfaces --- .../ompl/ompl_motion_planner.h | 12 ++++- .../profile/ompl_composite_profile_rvss.h | 14 ++++-- .../ompl/profile/ompl_waypoint_profile.h | 17 ++++--- .../ompl/src/ompl_motion_planner.cpp | 17 +++---- .../profile/ompl_composite_profile_rvss.cpp | 10 ++--- .../src/profile/ompl_waypoint_profile.cpp | 22 ++++----- .../test/ompl_planner_tests.cpp | 45 +++++++------------ 7 files changed, 73 insertions(+), 64 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index b0343bcf90e..7931eee035f 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -85,15 +85,23 @@ using OMPLCompositeProfileData = std::tuple +class OMPLPlannerProfile : public PlannerProfile { +public: OMPLPlannerParameters params; - inline OMPLPlannerParameters create() const override + inline std::any create() const override { if (params.planners.empty()) throw std::runtime_error("No OMPL planner factories defined"); return params; }; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } }; /** diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h index 899923aaa00..021a7066373 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_composite_profile_rvss.h @@ -28,8 +28,9 @@ using OptimizationObjectiveAllocator = /** * @brief OMPL composite profile for real-vector state spaces */ -struct OMPLCompositeProfileRVSS : public CompositeProfile +class OMPLCompositeProfileRVSS : public CompositeProfile { +public: /** * @brief The collision check configuration * @details Different validators will be created depending on the type of collision checking requested @@ -68,8 +69,8 @@ struct OMPLCompositeProfileRVSS : public CompositeProfile + void serialize(Archive&, const unsigned int) + { + } }; } // namespace tesseract_planning diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h index 345da98e185..ca8348368ca 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h @@ -5,12 +5,19 @@ namespace tesseract_planning { -struct OMPLWaypointProfile : public WaypointProfile> +class OMPLWaypointProfile : public WaypointProfile { - virtual std::vector create(const Instruction& instruction, - tesseract_environment::Environment::ConstPtr env) const override; +public: + virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } }; -} // namespace tesseract_planning +} // namespace tesseract_planning -#endif // TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H +#endif // TESSERACT_PLANNING_OMPL_PROFILE_OMPL_WAYPOINT_PROFILE_H diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 5134d3afb1f..3f75e0161c7 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -309,8 +309,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ { const std::string profile_name = getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); - auto pp = request.profiles->getProfile>(name_, profile_name); - params = pp->create(); + PlannerProfile::ConstPtr pp = request.profiles->planner_profiles.at(name_).at(profile_name); + params = std::any_cast(pp->create()); } catch (const std::exception&) { @@ -326,8 +326,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ { const std::string profile_name = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - auto cp = request.profiles->getProfile>(name_, profile_name); - std::tie(simple_setup, extractor) = cp->create(request.instructions, request.env); + CompositeProfile::ConstPtr cp = request.profiles->composite_profiles.at(name_).at(profile_name); + std::tie(simple_setup, extractor) = std::any_cast(cp->create(request.instructions, *request.env)); } catch (const std::exception& ex) { @@ -354,8 +354,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ { const PlanInstruction& pi = request.instructions.getStartInstruction().as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - auto p = request.profiles->getProfile>>(name_, profile_name); - start_states = p->create(pi, request.env); + WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); + start_states = std::any_cast>(p->create(pi, *request.env)); } else { @@ -377,9 +377,10 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ const PlanInstruction& pi = request.instructions[i].as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - auto p = request.profiles->getProfile>>(name_, profile_name); + WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); - auto states = createOMPLStates(p->create(pi, request.env), simple_setup->getSpaceInformation()); + auto states = createOMPLStates(std::any_cast>(p->create(pi, *request.env)), + simple_setup->getSpaceInformation()); auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); std::for_each(states.begin(), states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index 88c3e413db3..b3b2352ccce 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -16,8 +16,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruction& instruction, - tesseract_environment::Environment::ConstPtr env) const +std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instruction, + const tesseract_environment::Environment& env) const { for (const Instruction& inst : instruction) { @@ -28,7 +28,7 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc // Get the joint group information from the composite instruction and environment const tesseract_kinematics::JointGroup::ConstPtr joint_group = - env->getJointGroup(instruction.getManipulatorInfo().manipulator); + env.getJointGroup(instruction.getManipulatorInfo().manipulator); const std::vector joint_names = joint_group->getJointNames(); const Eigen::MatrixX2d limits = joint_group->getLimits().joint_limits; const Eigen::Index dof = joint_group->numJoints(); @@ -82,7 +82,7 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc case tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE: { auto svc = std::make_shared( - simple_setup->getSpaceInformation(), *env, joint_group, collision_check_config, extractor); + simple_setup->getSpaceInformation(), env, joint_group, collision_check_config, extractor); csvc->addStateValidator(svc); break; } @@ -117,7 +117,7 @@ OMPLCompositeProfileData OMPLCompositeProfileRVSS::create(const CompositeInstruc { mv = std::make_shared(simple_setup->getSpaceInformation(), custom_validity_checker, - *env, + env, joint_group, collision_check_config, extractor); diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp index 278bd73baad..9657e5176ef 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -4,10 +4,10 @@ namespace tesseract_planning { void checkCollision(const Eigen::VectorXd& state, - tesseract_environment::Environment::ConstPtr env, + const tesseract_environment::Environment& env, tesseract_kinematics::JointGroup::ConstPtr manip) { - tesseract_collision::DiscreteContactManager::Ptr contact_checker = env->getDiscreteContactManager(); + tesseract_collision::DiscreteContactManager::Ptr contact_checker = env.getDiscreteContactManager(); tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); for (const auto& link_name : contact_checker->getActiveCollisionObjects()) @@ -40,7 +40,7 @@ Eigen::VectorXd updateLimits(Eigen::Ref joint_waypoint, t tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& cartesian_waypoint, const ManipulatorInfo& mi, - tesseract_environment::Environment::ConstPtr env) + const tesseract_environment::Environment& env) { if (mi.manipulator.empty()) throw std::runtime_error("OMPL: manipulator is empty!"); @@ -53,9 +53,9 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c // Get the kinematics group for solving IK tesseract_kinematics::KinematicGroup::ConstPtr manip = - env->getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); + env.getKinematicGroup(mi.manipulator, mi.manipulator_ik_solver); - Eigen::Isometry3d tcp_offset = env->findTCPOffset(mi); + Eigen::Isometry3d tcp_offset = env.findTCPOffset(mi); Eigen::Isometry3d tcp_frame_cwp = cartesian_waypoint * tcp_offset.inverse(); tesseract_kinematics::KinGroupIKInput ik_input(tcp_frame_cwp, mi.working_frame, mi.tcp_frame); const tesseract_kinematics::IKSolutions joint_solutions = @@ -83,7 +83,7 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c solution, limits.joint_limits, manip->getRedundancyCapableJointIndices()); valid_solutions.insert(valid_solutions.end(), redundant_solutions.begin(), redundant_solutions.end()); } - catch(const std::exception& ex) + catch (const std::exception& ex) { CONSOLE_BRIDGE_logDebug(ex.what()); continue; @@ -96,8 +96,8 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c return valid_solutions; } -std::vector OMPLWaypointProfile::create(const Instruction& instruction, - tesseract_environment::Environment::ConstPtr env) const +std::any OMPLWaypointProfile::create(const Instruction& instruction, + const tesseract_environment::Environment& env) const { PlanInstruction plan_instruction = instruction.as(); tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo(); @@ -111,18 +111,18 @@ std::vector OMPLWaypointProfile::create(const Instruction& inst else if (isJointWaypoint(waypoint)) { const JointWaypoint& jw = waypoint.as(); - const Eigen::VectorXd updated_state = updateLimits(jw, env->getJointGroup(mi.manipulator)->getLimits()); + const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits()); return { updated_state }; } else if (isStateWaypoint(waypoint)) { const StateWaypoint& sw = waypoint.as(); Eigen::Map state(sw.position.data(), sw.position.size()); - const Eigen::VectorXd updated_state = updateLimits(state, env->getJointGroup(mi.manipulator)->getLimits()); + const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits()); return { updated_state }; } throw std::runtime_error("Unsupported waypoint type"); } -} // namespace tesseract_planning +} // namespace tesseract_planning diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index 37e03bca16b..fee3df6e2fe 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -224,12 +224,9 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT // Add the profiles { auto d = std::make_unique(); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); - d->addProfile>>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); request.profiles = std::move(d); } @@ -286,12 +283,9 @@ TYPED_TEST(OMPLTestFixture, StartStateInCollision) // Add the profiles { auto d = std::make_unique(); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); - d->addProfile>>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); request.profiles = std::move(d); } @@ -334,12 +328,9 @@ TYPED_TEST(OMPLTestFixture, EndStateInCollision) // Add the profiles { auto d = std::make_unique(); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); - d->addProfile>>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); request.profiles = std::move(d); } @@ -388,12 +379,9 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) // Add the profiles { auto d = std::make_unique(); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); - d->addProfile>>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); request.profiles = std::move(d); } @@ -454,12 +442,9 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) // Add the profiles { auto d = std::make_unique(); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createPlannerProfile()); - d->addProfile>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, this->createCompositeProfile()); - d->addProfile>>( - OMPL_DEFAULT_NAMESPACE, this->profile_name_, std::make_shared()); + d->planner_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createPlannerProfile(); + d->composite_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = this->createCompositeProfile(); + d->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][this->profile_name_] = std::make_shared(); request.profiles = std::move(d); } From 2d0d98c6c66a6964c18d52c1ebcdea5795e4ea67 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 16:28:57 -0600 Subject: [PATCH 30/46] Collision check joint and state waypoints --- .../ompl/src/profile/ompl_waypoint_profile.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp index 9657e5176ef..034738854ca 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -112,14 +112,16 @@ std::any OMPLWaypointProfile::create(const Instruction& instruction, { const JointWaypoint& jw = waypoint.as(); const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits()); - return { updated_state }; + checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); + return std::vector{ updated_state }; } else if (isStateWaypoint(waypoint)) { const StateWaypoint& sw = waypoint.as(); Eigen::Map state(sw.position.data(), sw.position.size()); const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits()); - return { updated_state }; + checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); + return std::vector{ updated_state }; } throw std::runtime_error("Unsupported waypoint type"); From c1d14ac9d55a413c959836ed70b09a5b60e6bbfb Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 16:30:36 -0600 Subject: [PATCH 31/46] Consolidated solve function under a single try-catch block --- .../ompl/src/ompl_motion_planner.cpp | 197 +++++++++--------- 1 file changed, 95 insertions(+), 102 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 3f75e0161c7..4b6e106cf9f 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -289,137 +289,130 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ PlannerResponse& response, bool verbose) const { - auto status_category_ = std::make_shared(name_); - - // Check the format of the request - if (!checkUserInput(request)) // NOLINT - { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } - // If the verbose set the log level to debug. if (verbose) console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_DEBUG); - // Get the planner profile - OMPLPlannerParameters params; - try - { - const std::string profile_name = - getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); - PlannerProfile::ConstPtr pp = request.profiles->planner_profiles.at(name_).at(profile_name); - params = std::any_cast(pp->create()); - } - catch (const std::exception&) - { - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } + auto status_category_ = std::make_shared(name_); - // Get the composite profile - ompl::geometric::SimpleSetupPtr simple_setup; - OMPLStateExtractor extractor; try { - const std::string profile_name = - getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - CompositeProfile::ConstPtr cp = request.profiles->composite_profiles.at(name_).at(profile_name); - std::tie(simple_setup, extractor) = std::any_cast(cp->create(request.instructions, *request.env)); - } - catch (const std::exception& ex) - { - CONSOLE_BRIDGE_logError(ex.what()); - response.status = - tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorInvalidInput, status_category_); - return response.status; - } + // Check the format of the request + if (!checkUserInput(request)) // NOLINT + throw std::runtime_error("Input is invalid"); - // Set up the output trajectory to be a composite of composites - response.results.reserve(request.instructions.size()); + // Get the planner profile + OMPLPlannerParameters params; + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); + PlannerProfile::ConstPtr pp = request.profiles->planner_profiles.at(name_).at(profile_name); + params = std::any_cast(pp->create()); + } - // Loop over each pair of waypoints - for (std::size_t i = 0; i < request.instructions.size(); ++i) - { - simple_setup->clearStartStates(); + // Get the composite profile + ompl::geometric::SimpleSetupPtr simple_setup; + OMPLStateExtractor extractor; + { + const std::string profile_name = + getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); + CompositeProfile::ConstPtr cp = request.profiles->composite_profiles.at(name_).at(profile_name); + std::tie(simple_setup, extractor) = + std::any_cast(cp->create(request.instructions, *request.env)); + } + + // Set up the output trajectory to be a composite of composites + response.results.reserve(request.instructions.size()); - // Add the start state(s) + // Loop over each pair of waypoints + for (std::size_t i = 0; i < request.instructions.size(); ++i) { - std::vector start_states; + simple_setup->clearStartStates(); - // Get the start waypoint profile and add the states to the SimpleSetup - if (i == 0) + // Add the start state(s) { - const PlanInstruction& pi = request.instructions.getStartInstruction().as(); - const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); - start_states = std::any_cast>(p->create(pi, *request.env)); + std::vector start_states; + + // Get the start waypoint profile and add the states to the SimpleSetup + if (i == 0) + { + const PlanInstruction& pi = request.instructions.getStartInstruction().as(); + const std::string profile_name = + getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); + start_states = std::any_cast>(p->create(pi, *request.env)); + } + else + { + // Use the last state of the previous trajectory as the single start state for this plan + const MoveInstruction& mi = response.results.back().as().back().as(); + const StateWaypoint& sw = mi.getWaypoint().as(); + start_states.push_back(sw.position); + } + + // Add the states to the SimpleSetup + auto states = createOMPLStates(start_states, simple_setup->getSpaceInformation()); + std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { + simple_setup->addStartState(state); + }); } - else + + // Add the goal waypoint(s) { - // Use the last state of the previous trajectory as the single start state for this plan - const MoveInstruction& mi = response.results.back().as().back().as(); - const StateWaypoint& sw = mi.getWaypoint().as(); - start_states.push_back(sw.position); - } + const PlanInstruction& pi = request.instructions[i].as(); - // Add the states to the SimpleSetup - auto states = createOMPLStates(start_states, simple_setup->getSpaceInformation()); - std::for_each(states.begin(), states.end(), [&simple_setup](const ompl::base::ScopedState<>& state) { - simple_setup->addStartState(state); - }); - } + const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); + WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); - // Add the goal waypoint(s) - { - const PlanInstruction& pi = request.instructions[i].as(); + std::vector states = std::any_cast>(p->create(pi, *request.env)); + auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation()); - const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); + auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); + std::for_each(ompl_states.begin(), ompl_states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { + goal_states->addState(state); + }); - auto states = createOMPLStates(std::any_cast>(p->create(pi, *request.env)), - simple_setup->getSpaceInformation()); + simple_setup->setGoal(goal_states); + } - auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); - std::for_each(states.begin(), states.end(), [&goal_states](const ompl::base::ScopedState<>& state) { - goal_states->addState(state); - }); + // The number of states in the seed is the size of the composite instruction plus one for the start state + const unsigned n_seed_states = static_cast(request.seed.at(i).as().size()) + 1; - simple_setup->setGoal(goal_states); - } + // Plan + auto planner_data = plan(simple_setup, params, n_seed_states); - // The number of states in the seed is the size of the composite instruction plus one for the start state - const unsigned n_seed_states = static_cast(request.seed.at(i).as().size()) + 1; + // Save the combined planner data in the response + // response.data = std::static_pointer_cast(planner_data); - // Plan - auto planner_data = plan(simple_setup, params, n_seed_states); + // Get the results + tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); + assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); + assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); - // Get the results - tesseract_common::TrajArray trajectory = toTrajArray(simple_setup->getSolutionPath(), extractor); - assert(checkStartState(simple_setup->getProblemDefinition(), trajectory.row(0), extractor)); - assert(checkGoalState(simple_setup->getProblemDefinition(), trajectory.bottomRows(1).transpose(), extractor)); + // Enforce limits + { + const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; + auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; + for (Eigen::Index i = 0; i < trajectory.rows(); i++) + tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); + } - // Enforce limits - { - const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - auto joint_limits = request.env->getJointGroup(manipulator)->getLimits().joint_limits; - for (Eigen::Index i = 0; i < trajectory.rows(); i++) - tesseract_common::enforcePositionLimits(trajectory.row(i), joint_limits); + // Construct the output trajectory instruction and add it to the response + response.results.push_back(buildTrajectoryInstruction(trajectory, request.seed[i].as())); } - // Construct the output trajectory instruction and add it to the response - response.results.push_back(buildTrajectoryInstruction(trajectory, request.seed[i].as())); - } - - // Set top-level composite start instruction to first waypoint of first trajectory - response.results.getStartInstruction() = response.results.at(0).as().getStartInstruction(); + // Set top-level composite start instruction to first waypoint of first trajectory + response.results.getStartInstruction() = response.results.at(0).as().getStartInstruction(); - // Save the combined planner data in the response -// response.data = std::static_pointer_cast(planner_data); + response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); + } + catch (const std::exception& ex) + { + CONSOLE_BRIDGE_logError(ex.what()); + response.status = + tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::ErrorFailedToFindValidSolution, status_category_); + } - response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); return response.status; } From ba029a9c19a021d4136a350f6aff0e0f411c22a1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 19:33:39 -0600 Subject: [PATCH 32/46] Clear simple setup data to solve correctly for subsequent plans --- tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 4b6e106cf9f..db7655ea081 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -328,6 +328,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ for (std::size_t i = 0; i < request.instructions.size(); ++i) { simple_setup->clearStartStates(); + simple_setup->clear(); // Add the start state(s) { From ca4b42008ca8bde892dd25f0e3c2b29221b7f0e5 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 19:34:02 -0600 Subject: [PATCH 33/46] Simplify logic --- tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index db7655ea081..f2d7ff2ac55 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -47,7 +47,7 @@ bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def, const Eigen::Ref& state, const OMPLStateExtractor& extractor) { - if (!(prob_def->getStartStateCount() >= 1)) + if (prob_def->getStartStateCount() < 1) return false; for (unsigned i = 0; i < prob_def->getStartStateCount(); ++i) From d9bf7fdf982d169ff4991f1b21ea03ac6f2cdd97 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 19:54:48 -0600 Subject: [PATCH 34/46] Added second waypoint to OMPL test programs --- .../examples/freespace_example.cpp | 52 +++++++++++-------- .../test/ompl_planner_tests.cpp | 43 +++++++++------ 2 files changed, 57 insertions(+), 38 deletions(-) diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 7490c45c6ba..6ab4c22dd01 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -29,25 +29,22 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include - #include - #include - -#include +#include +#include +#include +#include +#include +// OMPL #include - +#include +#include +// TrajOpt #include #include #include -#include -#include -#include - -#include -#include - using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; @@ -89,7 +86,7 @@ int main(int /*argc*/, char** /*argv*/) tesseract_common::fs::path srdf_path(std::string(TESSERACT_SUPPORT_DIR) + "/urdf/abb_irb2400.srdf"); env->init(urdf_path, srdf_path, locator); - // Dynamically load ignition visualizer if exist + // Dynamically load ignition visualizer if it exists tesseract_visualization::VisualizationLoader loader; auto plotter = loader.get(); @@ -132,18 +129,19 @@ int main(int /*argc*/, char** /*argv*/) } // Create Profiles - auto ompl_plan_profile = std::make_shared(); - auto trajopt_plan_profile = std::make_shared(); - auto trajopt_composite_profile = std::make_shared(); - - // Create a seed - CompositeInstruction seed = generateSeed(program, cur_state, env); + auto ompl_composite_profile = std::make_shared(); + auto ompl_planner_profile = std::make_shared(); + ompl_planner_profile->params.planners.push_back(std::make_shared()); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile>( + OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_planner_profile); + profiles->addProfile>( + OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_composite_profile); + + // Create a seed + CompositeInstruction seed = generateSeed(program, cur_state, env); // Create Planning Request PlannerRequest request; @@ -166,7 +164,15 @@ int main(int /*argc*/, char** /*argv*/) plotter->plotTrajectory(toJointTrajectory(ompl_response.results), *state_solver); } - // Update Seed + // Create the TrajOpt profiles + auto trajopt_plan_profile = std::make_shared(); + auto trajopt_composite_profile = std::make_shared(); + + // Add the TrajOpt profiles to the dictionary + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + + // Update the seed to be the OMPL trajecory request.seed = ompl_response.results; // Solve TrajOpt Plan diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index fee3df6e2fe..bc239187bf1 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -144,7 +144,7 @@ class OMPLTestFixture : public ::testing::Test std::shared_ptr createPlannerProfile() { auto planner_profile = std::make_shared(); - planner_profile->params.planning_time = 10.0; + planner_profile->params.planning_time = 5.0; planner_profile->params.optimize = false; planner_profile->params.max_solutions = 2; planner_profile->params.simplify = false; @@ -206,6 +206,7 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT // Define Plan Instructions const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -213,6 +214,7 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT program.setStartInstruction(start_instruction); program.setManipulatorInfo(this->manip); program.push_back(plan_f1); + program.push_back(plan_f2); // Create Planner Request PlannerRequest request; @@ -240,8 +242,8 @@ TYPED_TEST(OMPLTestFixture, JointStartJointGoal) // NOLINT ASSERT_TRUE(&status); EXPECT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); - EXPECT_EQ(planner_response.results.size(), 1); + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); + EXPECT_EQ(planner_response.results.size(), 2); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); EXPECT_TRUE(wp2.isApprox( getJointPosition( @@ -361,6 +363,7 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) // Define Plan Instructions const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -368,6 +371,7 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) program.setStartInstruction(start_instruction); program.setManipulatorInfo(this->manip); program.push_back(plan_f1); + program.push_back(plan_f2); // Create Planner Request PlannerRequest request; @@ -394,12 +398,14 @@ TYPED_TEST(OMPLTestFixture, JointStartCartesianGoal) } ASSERT_TRUE(&status); ASSERT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); EXPECT_TRUE(wp1.isApprox(getJointPosition(getFirstMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); + EXPECT_TRUE(wp1.isApprox(getJointPosition(getLastMoveInstruction(planner_response.results)->getWaypoint()), 1e-5)); - const MoveInstruction* last_move = getLastMoveInstruction(planner_response.results); - const Eigen::VectorXd& last_move_joints = getJointPosition(last_move->getWaypoint()); - const Eigen::Isometry3d check_goal = kin_group->calcFwdKin(last_move_joints).at(this->manip.tcp_frame); + const MoveInstruction* cart_move = + getLastMoveInstruction(planner_response.results.front().as()); + const Eigen::VectorXd& cart_move_joints = getJointPosition(cart_move->getWaypoint()); + const Eigen::Isometry3d check_goal = kin_group->calcFwdKin(cart_move_joints).at(this->manip.tcp_frame); EXPECT_TRUE(wp2.isApprox(check_goal, 1e-3)); } @@ -424,6 +430,7 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) // Define Plan Instructions const PlanInstruction plan_f1(wp2, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); + const PlanInstruction plan_f2(wp1, PlanInstructionType::FREESPACE, this->profile_name_, this->manip); // Create a program CompositeInstruction program; @@ -431,6 +438,7 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) program.setStartInstruction(start_instruction); program.setManipulatorInfo(this->manip); program.push_back(plan_f1); + program.push_back(plan_f2); // Create Planner Request PlannerRequest request; @@ -458,15 +466,20 @@ TYPED_TEST(OMPLTestFixture, CartesianStartJointGoal) ASSERT_TRUE(&status); ASSERT_TRUE(planner_response.results.hasStartInstruction()); - EXPECT_GE(getMoveInstructionCount(planner_response.results), this->seed_steps_); - - const MoveInstruction* last_mi = getLastMoveInstruction(planner_response.results); + EXPECT_GE(getMoveInstructionCount(planner_response.results), 2 * this->seed_steps_ + 1); + + // Check the start/end Cartesian coordinate against the target waypoint + auto check_cartesian_pose = [&](const MoveInstruction* mi) { + const Eigen::VectorXd& joints = getJointPosition(mi->getWaypoint()); + const Eigen::Isometry3d pose = kin_group->calcFwdKin(joints).at(this->manip.tcp_frame); + EXPECT_TRUE(wp1.isApprox(pose, 1e-3)); + }; + check_cartesian_pose(getFirstMoveInstruction(planner_response.results)); + check_cartesian_pose(getLastMoveInstruction(planner_response.results)); + + // Check the joint move + const MoveInstruction* last_mi = getLastMoveInstruction(planner_response.results.front().as()); EXPECT_TRUE(wp2.isApprox(getJointPosition(last_mi->getWaypoint()), 1e-5)); - - const MoveInstruction* first_mi = getFirstMoveInstruction(planner_response.results); - const Eigen::VectorXd& joints = getJointPosition(first_mi->getWaypoint()); - const Eigen::Isometry3d check_start = kin_group->calcFwdKin(joints).at(this->manip.tcp_frame); - EXPECT_TRUE(wp1.isApprox(check_start, 1e-3)); } // TEST(OMPLMultiPlanner, OMPLMultiPlannerUnit) // NOLINT From 638416a1620cff732840c0003fbcef43997aa924 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 7 Jan 2022 19:55:29 -0600 Subject: [PATCH 35/46] Updated freespace example --- .../examples/freespace_example.cpp | 63 ++++++++++++------- 1 file changed, 41 insertions(+), 22 deletions(-) diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 6ab4c22dd01..2bd42adecee 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP // OMPL #include #include +#include #include // TrajOpt #include @@ -48,6 +49,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP using namespace tesseract_planning; using namespace tesseract_planning::profile_ns; +static const std::string PROFILE_NAME = "DEFAULT"; + std::string locateResource(const std::string& url) { std::string mod_url = url; @@ -75,7 +78,31 @@ std::string locateResource(const std::string& url) return mod_url; } -int main(int /*argc*/, char** /*argv*/) +std::shared_ptr createOMPLCompositeProfile() +{ + auto composite_profile = std::make_shared(); + composite_profile->collision_check_config.contact_manager_config.margin_data_override_type = + tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; + composite_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); + composite_profile->collision_check_config.longest_valid_segment_length = 0.1; + composite_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; + + return composite_profile; +} + +std::shared_ptr createOMPLPlannerProfile() +{ + auto planner_profile = std::make_shared(); + planner_profile->params.planning_time = 10.0; + planner_profile->params.optimize = false; + planner_profile->params.max_solutions = 2; + planner_profile->params.simplify = false; + planner_profile->params.planners = { std::make_shared(), + std::make_shared() }; + return planner_profile; +} + +int main(int argc, char** argv) { try { @@ -88,7 +115,8 @@ int main(int /*argc*/, char** /*argv*/) // Dynamically load ignition visualizer if it exists tesseract_visualization::VisualizationLoader loader; - auto plotter = loader.get(); + const std::string plugin_name = argc < 2 ? "" : argv[1]; + auto plotter = loader.get(plugin_name); if (plotter != nullptr) { @@ -114,31 +142,22 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0); // Define Plan Instructions - PlanInstruction start_instruction(wp0, PlanInstructionType::START); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, "DEFAULT"); + PlanInstruction start_instruction(wp0, PlanInstructionType::START, PROFILE_NAME, manip); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, PROFILE_NAME, manip); + PlanInstruction plan_f2(wp0, PlanInstructionType::FREESPACE, PROFILE_NAME, manip); // Create program CompositeInstruction program; program.setStartInstruction(start_instruction); program.setManipulatorInfo(manip); program.push_back(plan_f1); - - // Plot Program - if (plotter) - { - } - - // Create Profiles - auto ompl_composite_profile = std::make_shared(); - auto ompl_planner_profile = std::make_shared(); - ompl_planner_profile->params.planners.push_back(std::make_shared()); + program.push_back(plan_f2); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>( - OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_planner_profile); - profiles->addProfile>( - OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_composite_profile); + profiles->planner_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = createOMPLPlannerProfile(); + profiles->composite_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = createOMPLCompositeProfile(); + profiles->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); // Create a seed CompositeInstruction seed = generateSeed(program, cur_state, env); @@ -158,7 +177,7 @@ int main(int /*argc*/, char** /*argv*/) assert(ompl_status); // Plot OMPL Trajectory - if (plotter) + if (plotter != nullptr) { plotter->waitForInput(); plotter->plotTrajectory(toJointTrajectory(ompl_response.results), *state_solver); @@ -169,8 +188,8 @@ int main(int /*argc*/, char** /*argv*/) auto trajopt_composite_profile = std::make_shared(); // Add the TrajOpt profiles to the dictionary - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE_NAME, trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, PROFILE_NAME, trajopt_composite_profile); // Update the seed to be the OMPL trajecory request.seed = ompl_response.results; @@ -178,7 +197,7 @@ int main(int /*argc*/, char** /*argv*/) // Solve TrajOpt Plan PlannerResponse trajopt_response; TrajOptMotionPlanner trajopt_planner; - auto trajopt_status = trajopt_planner.solve(request, trajopt_response); + auto trajopt_status = trajopt_planner.solve(request, trajopt_response, true); assert(trajopt_status); if (plotter) From 00da67cb2435144a9d47b1daa5a3afd6fd350dfe Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 18:47:57 -0600 Subject: [PATCH 36/46] Added convenience function for getting waypoint, composite, and planner profiles --- .../profile_dictionary.h | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index 01f0fab6d5e..a5aa1ca5d03 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -30,11 +30,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include #include #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #ifdef SWIG @@ -307,7 +309,54 @@ class ProfileDictionary std::unordered_map> composite_profiles; std::unordered_map> planner_profiles; + WaypointProfile::ConstPtr getWaypointProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, waypoint_profiles); + } + + CompositeProfile::ConstPtr getCompositeProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, composite_profiles); + } + + PlannerProfile::ConstPtr getPlannerProfile(const std::string& ns, const std::string& profile_name) const + { + return getProfile(ns, profile_name, planner_profiles); + } + protected: + template + typename ProfileT::ConstPtr getProfile( + const std::string& ns, + const std::string& profile_name, + const std::unordered_map>& map) const + { + try + { + return map.at(ns).at(profile_name); + } + catch (const std::out_of_range&) + { + std::stringstream ss; + ss << "Failed to get " << boost::core::demangle(typeid(ProfileT).name()) << " for '" << ns << "/" << profile_name + << "'\n"; + if (map.find(ns) == map.end()) + { + ss << "Profile namespace '" << ns << "' does not exist"; + } + else + { + ss << "Entries in profile namespace '" << ns << "' are:"; + for (auto it = map.at(ns).begin(); it != map.at(ns).end(); ++it) + { + ss << "\n\t" << it->first; + } + } + + throw std::out_of_range(ss.str()); + } + } + std::unordered_map> profiles_; mutable std::shared_mutex mutex_; }; From 637edca303b74e3c2fedaf83f0cb645ea6675681 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 18:48:12 -0600 Subject: [PATCH 37/46] Use helper function in OMPL planner to get profiles --- .../ompl/src/ompl_motion_planner.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index f2d7ff2ac55..48b56499f41 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -306,7 +306,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ { const std::string profile_name = getProfileString(name_, request.instructions.getProfile(), request.plan_profile_remapping); - PlannerProfile::ConstPtr pp = request.profiles->planner_profiles.at(name_).at(profile_name); + PlannerProfile::ConstPtr pp = request.profiles->getPlannerProfile(name_, profile_name); params = std::any_cast(pp->create()); } @@ -316,7 +316,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ { const std::string profile_name = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - CompositeProfile::ConstPtr cp = request.profiles->composite_profiles.at(name_).at(profile_name); + CompositeProfile::ConstPtr cp = request.profiles->getCompositeProfile(name_, profile_name); std::tie(simple_setup, extractor) = std::any_cast(cp->create(request.instructions, *request.env)); } @@ -340,7 +340,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ const PlanInstruction& pi = request.instructions.getStartInstruction().as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); + WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); start_states = std::any_cast>(p->create(pi, *request.env)); } else @@ -363,7 +363,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ const PlanInstruction& pi = request.instructions[i].as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); - WaypointProfile::ConstPtr p = request.profiles->waypoint_profiles.at(name_).at(profile_name); + WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); std::vector states = std::any_cast>(p->create(pi, *request.env)); auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation()); From 87646acedbb5ad3b0d154aa507d914f92dda2937 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 18:51:08 -0600 Subject: [PATCH 38/46] Copied instruction meta-data from request into response --- tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 48b56499f41..f2c072a94da 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -321,6 +321,11 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ std::any_cast(cp->create(request.instructions, *request.env)); } + // Copy the meta-data from the request instruction into the response and clear any child instructions from the + // response + response.results = request.instructions; + response.results.clear(); + // Set up the output trajectory to be a composite of composites response.results.reserve(request.instructions.size()); @@ -403,7 +408,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ } // Set top-level composite start instruction to first waypoint of first trajectory - response.results.getStartInstruction() = response.results.at(0).as().getStartInstruction(); + response.results.setStartInstruction(response.results.at(0).as().getStartInstruction()); response.status = tesseract_common::StatusCode(OMPLMotionPlannerStatusCategory::SolutionFound, status_category_); } From cf6cff4ec7a8de1611daa919dbce342f6169973c Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:10:47 -0600 Subject: [PATCH 39/46] Moved OMPL planner profile to profile directory --- .../examples/freespace_example.cpp | 30 ++--------------- .../ompl/ompl_motion_planner.h | 22 ------------- .../ompl/profile/ompl_planner_profile.h | 33 +++++++++++++++++++ .../test/ompl_planner_tests.cpp | 2 +- 4 files changed, 37 insertions(+), 50 deletions(-) create mode 100644 tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index 2bd42adecee..7a0ee6208d2 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -38,9 +38,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include // OMPL #include +#include #include #include -#include // TrajOpt #include #include @@ -78,30 +78,6 @@ std::string locateResource(const std::string& url) return mod_url; } -std::shared_ptr createOMPLCompositeProfile() -{ - auto composite_profile = std::make_shared(); - composite_profile->collision_check_config.contact_manager_config.margin_data_override_type = - tesseract_collision::CollisionMarginOverrideType::OVERRIDE_DEFAULT_MARGIN; - composite_profile->collision_check_config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.025); - composite_profile->collision_check_config.longest_valid_segment_length = 0.1; - composite_profile->collision_check_config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - - return composite_profile; -} - -std::shared_ptr createOMPLPlannerProfile() -{ - auto planner_profile = std::make_shared(); - planner_profile->params.planning_time = 10.0; - planner_profile->params.optimize = false; - planner_profile->params.max_solutions = 2; - planner_profile->params.simplify = false; - planner_profile->params.planners = { std::make_shared(), - std::make_shared() }; - return planner_profile; -} - int main(int argc, char** argv) { try @@ -155,8 +131,8 @@ int main(int argc, char** argv) // Profile Dictionary auto profiles = std::make_shared(); - profiles->planner_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = createOMPLPlannerProfile(); - profiles->composite_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = createOMPLCompositeProfile(); + profiles->planner_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); + profiles->composite_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); profiles->waypoint_profiles[OMPL_DEFAULT_NAMESPACE][PROFILE_NAME] = std::make_shared(); // Create a seed diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 7931eee035f..5f535bb944b 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -82,28 +82,6 @@ using OMPLStateExtractor = std::function(const ompl: */ using OMPLCompositeProfileData = std::tuple; -/** - * @brief Planner profile that produces the high level parameters for the OMPL planner - */ -class OMPLPlannerProfile : public PlannerProfile -{ -public: - OMPLPlannerParameters params; - inline std::any create() const override - { - if (params.planners.empty()) - throw std::runtime_error("No OMPL planner factories defined"); - return params; - }; - -private: - friend class boost::serialization::access; - template - void serialize(Archive&, const unsigned int) - { - } -}; - /** * @brief This planner is intended to provide an easy to use interface to OMPL for freespace planning. It is made to * take a start and end point and automate the generation of the OMPL problem. diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h new file mode 100644 index 00000000000..5cbd6b8ec06 --- /dev/null +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_planner_profile.h @@ -0,0 +1,33 @@ +#ifndef TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_PROFILE_H + +#include +#include + +namespace tesseract_planning +{ +/** + * @brief Planner profile that produces the high level parameters for the OMPL planner + */ +class OMPLPlannerProfile : public PlannerProfile +{ +public: + OMPLPlannerParameters params; + inline std::any create() const override + { + if (params.planners.empty()) + throw std::runtime_error("No OMPL planner factories defined"); + return params; + }; + +private: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int) + { + } +}; + +} // namespace tesseract_planning + +#endif // TESSERACT_MOTION_PLANNERS_OMPL_PROFILE_OMPL_PLANNER_PROFILE_H diff --git a/tesseract_motion_planners/test/ompl_planner_tests.cpp b/tesseract_motion_planners/test/ompl_planner_tests.cpp index bc239187bf1..bc49d7bbe06 100644 --- a/tesseract_motion_planners/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/test/ompl_planner_tests.cpp @@ -37,7 +37,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include From 75eedeefdc04a6a0ca5b5430a93927ac43fce58f Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:12:00 -0600 Subject: [PATCH 40/46] Updated default planner parameters for OMPL --- .../tesseract_motion_planners/ompl/ompl_motion_planner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h index 5f535bb944b..8efed26a6c6 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h @@ -68,10 +68,10 @@ struct OMPLPlannerParameters * to true it will leverage all planning time to keep finding solutions up to your max solutions count to find the * most optimal solution. */ - bool optimize = true; + bool optimize = false; /** @brief OMPL planning factories for creating OMPL planners to solve the planning problem */ - std::vector planners; + std::vector planners = { std::make_shared() }; }; /** @brief Function signature for extracting a Tesseract state representations from an OMPL state representation */ From 9d5141bab52fba19fd7e5c643d068398a147c4f6 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:13:15 -0600 Subject: [PATCH 41/46] Added manipulator info to all instructions in example programs --- .../examples/freespace_example_program.h | 20 ++--- .../examples/raster_dt_example_program.h | 58 ++++++------- .../examples/raster_example_program.h | 86 +++++++++---------- .../examples/raster_waad_dt_example_program.h | 74 ++++++++-------- .../examples/raster_waad_example_program.h | 62 ++++++------- 5 files changed, 150 insertions(+), 150 deletions(-) diff --git a/tesseract_process_managers/examples/freespace_example_program.h b/tesseract_process_managers/examples/freespace_example_program.h index f62ee2d5ef8..018eac8e76a 100644 --- a/tesseract_process_managers/examples/freespace_example_program.h +++ b/tesseract_process_managers/examples/freespace_example_program.h @@ -14,24 +14,24 @@ inline CompositeInstruction freespaceExampleProgramIIWA( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7" }; Waypoint wp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(7)); - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); // Define target pose Waypoint wp2 = CartesianWaypoint(goal); - PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("freespace_motion"); program.push_back(plan_f0); Waypoint wp3 = JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)); - PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE); + PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE, freespace_profile, manip); program.push_back(plan_f1); return program; @@ -42,23 +42,23 @@ inline CompositeInstruction freespaceExampleProgramABB( const std::string& composite_profile = DEFAULT_PROFILE_KEY, const std::string& freespace_profile = DEFAULT_PROFILE_KEY) { - CompositeInstruction program( - composite_profile, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(composite_profile, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; Waypoint wp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); // Define target pose Waypoint wp2 = CartesianWaypoint(goal); - PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp2, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("freespace_motion"); program.push_back(plan_f0); Waypoint wp3 = JointWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE); + PlanInstruction plan_f1(wp3, PlanInstructionType::FREESPACE, freespace_profile, manip); program.push_back(plan_f1); return program; diff --git a/tesseract_process_managers/examples/raster_dt_example_program.h b/tesseract_process_managers/examples/raster_dt_example_program.h index 6efcbaddeef..73289134112 100644 --- a/tesseract_process_managers/examples/raster_dt_example_program.h +++ b/tesseract_process_managers/examples/raster_dt_example_program.h @@ -36,20 +36,20 @@ namespace tesseract_planning inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); @@ -75,25 +75,25 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(freespace_profile); + CompositeInstruction raster_segment(freespace_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -108,21 +108,21 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp7_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); @@ -137,30 +137,30 @@ inline CompositeInstruction rasterDTExampleProgram(const std::string& freespace_ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp1_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end; + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); diff --git a/tesseract_process_managers/examples/raster_example_program.h b/tesseract_process_managers/examples/raster_example_program.h index a76532c9433..73a8bc1a94d 100644 --- a/tesseract_process_managers/examples/raster_example_program.h +++ b/tesseract_process_managers/examples/raster_example_program.h @@ -36,20 +36,20 @@ namespace tesseract_planning inline CompositeInstruction rasterExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start(freespace_profile); from_start.setDescription("from_start"); @@ -75,25 +75,25 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(process_profile); + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -104,10 +104,10 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -118,19 +118,19 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); @@ -141,14 +141,14 @@ inline CompositeInstruction rasterExampleProgram(const std::string& freespace_pr inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespace_profile = DEFAULT_PROFILE_KEY, const std::string& process_profile = "PROCESS") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define start instruction - PlanInstruction start_instruction(wp1, PlanInstructionType::START); + PlanInstruction start_instruction(wp1, PlanInstructionType::START, process_profile, manip); start_instruction.setDescription("Start Instruction"); program.setStartInstruction(start_instruction); @@ -170,25 +170,25 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac Waypoint wp7 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction raster_segment(process_profile); + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.setDescription("Raster #" + std::to_string(i + 1)); if (i == 0 || i == 2) { - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); } else { - raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + raster_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + raster_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); } program.push_back(raster_segment); @@ -199,10 +199,10 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -213,10 +213,10 @@ inline CompositeInstruction rasterOnlyExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); diff --git a/tesseract_process_managers/examples/raster_waad_dt_example_program.h b/tesseract_process_managers/examples/raster_waad_dt_example_program.h index dd92a9d79b1..59b1bf6970f 100644 --- a/tesseract_process_managers/examples/raster_waad_dt_example_program.h +++ b/tesseract_process_managers/examples/raster_waad_dt_example_program.h @@ -39,22 +39,22 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp const std::string& process_profile = "PROCESS", const std::string& departure_profile = "DEPARTURE") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); - CompositeInstruction from_start(freespace_profile); + CompositeInstruction from_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); from_start.setDescription("from_start"); from_start.push_back(plan_f0); program.push_back(from_start); @@ -84,45 +84,45 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp Waypoint wpd = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction approach_segment(approach_profile); - CompositeInstruction process_segment(process_profile); - CompositeInstruction departure_segment(departure_profile); + CompositeInstruction approach_segment(approach_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction process_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction departure_segment(departure_profile, CompositeInstructionOrder::ORDERED, manip); approach_segment.setDescription("Raster Approach #" + std::to_string(i + 1)); process_segment.setDescription("Raster Process #" + std::to_string(i + 1)); departure_segment.setDescription("Raster Departure #" + std::to_string(i + 1)); if (i == 0 || i == 2) { // Approach - approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile, manip)); // Process - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile, manip)); } else { // Approach - approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile, manip)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile, manip)); } - CompositeInstruction raster_segment; + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.push_back(approach_segment); raster_segment.push_back(process_segment); raster_segment.push_back(departure_segment); @@ -139,21 +139,21 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); @@ -168,30 +168,30 @@ inline CompositeInstruction rasterWAADDTExampleProgram(const std::string& freesp CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + (i * 0.1), 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1_dt(wp_dt, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1_dt.setDescription("transition_to_start_plan"); - CompositeInstruction transition_from_end(freespace_profile); + CompositeInstruction transition_from_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_from_end.setDescription("transition_from_end"); transition_from_end.push_back(plan_f1); - CompositeInstruction transition_to_start(freespace_profile); + CompositeInstruction transition_to_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition_to_start.setDescription("transition_to_start"); transition_to_start.push_back(plan_f1_dt); - CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transition("transition dual", CompositeInstructionOrder::UNORDERED, manip); transition.push_back(transition_from_end); transition.push_back(transition_to_start); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); diff --git a/tesseract_process_managers/examples/raster_waad_example_program.h b/tesseract_process_managers/examples/raster_waad_example_program.h index 91807b2c7c0..8e3b59f5756 100644 --- a/tesseract_process_managers/examples/raster_waad_example_program.h +++ b/tesseract_process_managers/examples/raster_waad_example_program.h @@ -39,22 +39,22 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac const std::string& process_profile = "PROCESS", const std::string& departure_profile = "DEPARTURE") { - CompositeInstruction program( - DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + const ManipulatorInfo manip("manipulator", "base_link", "tool0"); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program std::vector joint_names = { "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" }; StateWaypoint swp1 = StateWaypoint(joint_names, Eigen::VectorXd::Zero(6)); - PlanInstruction start_instruction(swp1, PlanInstructionType::START); + PlanInstruction start_instruction(swp1, PlanInstructionType::START, freespace_profile, manip); program.setStartInstruction(start_instruction); Waypoint wp1 = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); // Define from start composite instruction - PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f0(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f0.setDescription("from_start_plan"); - CompositeInstruction from_start(freespace_profile); + CompositeInstruction from_start(freespace_profile, CompositeInstructionOrder::ORDERED, manip); from_start.setDescription("from_start"); from_start.push_back(plan_f0); program.push_back(from_start); @@ -84,45 +84,45 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac Waypoint wpd = CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(x, 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - CompositeInstruction approach_segment(approach_profile); - CompositeInstruction process_segment(process_profile); - CompositeInstruction departure_segment(departure_profile); + CompositeInstruction approach_segment(approach_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction process_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction departure_segment(departure_profile, CompositeInstructionOrder::ORDERED, manip); approach_segment.setDescription("Raster Approach #" + std::to_string(i + 1)); process_segment.setDescription("Raster Process #" + std::to_string(i + 1)); departure_segment.setDescription("Raster Departure #" + std::to_string(i + 1)); if (i == 0 || i == 2) { // Approach - approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, approach_profile, manip)); // Process - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpd, PlanInstructionType::LINEAR, departure_profile, manip)); } else { // Approach - approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile)); + approach_segment.push_back(PlanInstruction(wp7, PlanInstructionType::LINEAR, approach_profile, manip)); - process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile)); - process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile)); + process_segment.push_back(PlanInstruction(wp6, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp5, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp4, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp3, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp2, PlanInstructionType::LINEAR, process_profile, manip)); + process_segment.push_back(PlanInstruction(wp1, PlanInstructionType::LINEAR, process_profile, manip)); // Departure - departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile)); + departure_segment.push_back(PlanInstruction(wpa, PlanInstructionType::LINEAR, departure_profile, manip)); } - CompositeInstruction raster_segment; + CompositeInstruction raster_segment(process_profile, CompositeInstructionOrder::ORDERED, manip); raster_segment.push_back(approach_segment); raster_segment.push_back(process_segment); raster_segment.push_back(departure_segment); @@ -135,10 +135,10 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), 0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp7, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); @@ -149,19 +149,19 @@ inline CompositeInstruction rasterWAADExampleProgram(const std::string& freespac CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8 + ((i + 1) * 0.1), -0.3, 0.85) * Eigen::Quaterniond(0, 0, -1.0, 0)); - PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f1(wp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f1.setDescription("transition_from_end_plan"); - CompositeInstruction transition(freespace_profile); + CompositeInstruction transition(freespace_profile, CompositeInstructionOrder::ORDERED, manip); transition.setDescription("transition_from_end"); transition.push_back(plan_f1); program.push_back(transition); } } - PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile); + PlanInstruction plan_f2(swp1, PlanInstructionType::FREESPACE, freespace_profile, manip); plan_f2.setDescription("to_end_plan"); - CompositeInstruction to_end(freespace_profile); + CompositeInstruction to_end(freespace_profile, CompositeInstructionOrder::ORDERED, manip); to_end.setDescription("to_end"); to_end.push_back(plan_f2); program.push_back(to_end); From e10eb470f5643da8f2f1b26e17bd4e52315f81b9 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:14:08 -0600 Subject: [PATCH 42/46] Added method to planning server for loading default profiles --- .../core/process_planning_server.h | 5 ++ .../src/core/process_planning_server.cpp | 53 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h b/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h index 9fd828cdcf3..f559acbeeb2 100644 --- a/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h +++ b/tesseract_process_managers/include/tesseract_process_managers/core/process_planning_server.h @@ -115,6 +115,11 @@ class ProcessPlanningServer */ std::vector getAvailableProcessPlanners() const; + /** + * @brief Loads the default profiles for the various motion planners + */ + void loadDefaultProfiles(); + #ifndef SWIG /** * @brief Execute a process planning request. diff --git a/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract_process_managers/src/core/process_planning_server.cpp index 53b1ff9bbb2..4b69c66d700 100644 --- a/tesseract_process_managers/src/core/process_planning_server.cpp +++ b/tesseract_process_managers/src/core/process_planning_server.cpp @@ -36,6 +36,23 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +// Default profiles +// Descartes +#include +// OMPL +#include +#include +#include +// Simple +#include +// Trajopt +#include +#include +#include +// Trajopt IFOPT +#include +#include + namespace tesseract_planning { ProcessPlanningServer::ProcessPlanningServer(EnvironmentCache::ConstPtr cache, size_t n) @@ -102,6 +119,42 @@ std::vector ProcessPlanningServer::getAvailableProcessPlanners() co return planners; } +void ProcessPlanningServer::loadDefaultProfiles() +{ + // Add TrajOpt Default profiles + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + profiles_->addProfile( + profile_ns::TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); + + // Add TrajOpt IFOPT Default profiles + profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared()); + profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared()); + + // Add Descartes Default profiles + profiles_->addProfile>(profile_ns::DESCARTES_DEFAULT_NAMESPACE, + DEFAULT_PROFILE_KEY, + std::make_shared>()); + + // Add OMPL Default profiles + profiles_->planner_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + profiles_->composite_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + profiles_->waypoint_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = + std::make_shared(); + + // Add Simple Default profiles + profiles_->addProfile( + profile_ns::SIMPLE_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, std::make_shared()); +} + ProcessPlanningFuture ProcessPlanningServer::run(const ProcessPlanningRequest& request) { CONSOLE_BRIDGE_logInform("Tesseract Planning Server Received Request!"); From e0ff36833c817672dec73af024612134729ec6f2 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:14:42 -0600 Subject: [PATCH 43/46] Update test to add default profiles to planning server --- .../src/core/process_planning_server.cpp | 12 ++++++------ .../test/tesseract_process_managers_unit.cpp | 14 ++++++++++++++ 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/tesseract_process_managers/src/core/process_planning_server.cpp b/tesseract_process_managers/src/core/process_planning_server.cpp index 4b69c66d700..d307ad032fa 100644 --- a/tesseract_process_managers/src/core/process_planning_server.cpp +++ b/tesseract_process_managers/src/core/process_planning_server.cpp @@ -131,16 +131,16 @@ void ProcessPlanningServer::loadDefaultProfiles() // Add TrajOpt IFOPT Default profiles profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, - DEFAULT_PROFILE_KEY, - std::make_shared()); + DEFAULT_PROFILE_KEY, + std::make_shared()); profiles_->addProfile(profile_ns::TRAJOPT_IFOPT_DEFAULT_NAMESPACE, - DEFAULT_PROFILE_KEY, - std::make_shared()); + DEFAULT_PROFILE_KEY, + std::make_shared()); // Add Descartes Default profiles profiles_->addProfile>(profile_ns::DESCARTES_DEFAULT_NAMESPACE, - DEFAULT_PROFILE_KEY, - std::make_shared>()); + DEFAULT_PROFILE_KEY, + std::make_shared>()); // Add OMPL Default profiles profiles_->planner_profiles[profile_ns::OMPL_DEFAULT_NAMESPACE][DEFAULT_PROFILE_KEY] = diff --git a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp index 4d4198f321b..2d0c10df5f7 100644 --- a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp +++ b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp @@ -336,6 +336,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultPlanProfileTest) // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -372,6 +373,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultLVSPlanProfileTes // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -408,6 +410,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultPlanProfile // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -444,6 +447,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultLVSPlanProf // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -480,6 +484,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultPlanProfileTe // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -516,6 +521,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfil // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -552,6 +558,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultPlanPro // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -588,6 +595,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlan // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -624,6 +632,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultPlanProfileTest // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -660,6 +669,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultLVSPlanProfileT // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -696,6 +706,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultPlanProfileTe // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -739,6 +750,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultLVSPlanProfil // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -782,6 +794,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultPlanProfile // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; @@ -825,6 +838,7 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultLVSPlanProf // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env_), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; From 57df7d489a0855c7a0314af6a41d91308bfbc389 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:48:40 -0600 Subject: [PATCH 44/46] Updated process managers test to use default profile names for all instructions since only the default profiles were added to the planning server --- .../test/tesseract_process_managers_unit.cpp | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp index 2d0c10df5f7..4eeee04f21a 100644 --- a/tesseract_process_managers/test/tesseract_process_managers_unit.cpp +++ b/tesseract_process_managers/test/tesseract_process_managers_unit.cpp @@ -139,7 +139,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerFixedSizeAssignPlan std::string freespace_profile = DEFAULT_PROFILE_KEY; std::string process_profile = "PROCESS"; - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); + tesseract_planning::CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); @@ -181,7 +181,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) std::string freespace_profile = DEFAULT_PROFILE_KEY; std::string process_profile = "PROCESS"; - tesseract_planning::CompositeInstruction program = rasterExampleProgram(); + tesseract_planning::CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); EXPECT_FALSE(program.getManipulatorInfo().empty()); program.setManipulatorInfo(manip); @@ -220,7 +220,7 @@ TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerLVSPlanProfileTest) TEST_F(TesseractProcessManagerUnit, RasterSimpleMotionPlannerDefaultLVSNoIKPlanProfileTest) // NOLINT { std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; tesseract_planning::CompositeInstruction program = rasterExampleProgram(); EXPECT_FALSE(program.getManipulatorInfo().empty()); @@ -344,7 +344,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultPlanProfileTest) // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -381,7 +381,7 @@ TEST_F(TesseractProcessManagerUnit, RasterProcessManagerDefaultLVSPlanProfileTes // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -418,7 +418,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultPlanProfile // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -455,7 +455,7 @@ TEST_F(TesseractProcessManagerUnit, RasterGlobalProcessManagerDefaultLVSPlanProf // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -492,7 +492,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultPlanProfileTe // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -529,7 +529,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfil // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -566,7 +566,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultPlanPro // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -603,7 +603,7 @@ TEST_F(TesseractProcessManagerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlan // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -640,7 +640,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultPlanProfileTest // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterDTExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -677,7 +677,7 @@ TEST_F(TesseractProcessManagerUnit, RasterDTProcessManagerDefaultLVSPlanProfileT // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string process_profile = "PROCESS"; + std::string process_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterDTExampleProgram(freespace_profile, process_profile); request.instructions = Instruction(program); @@ -714,9 +714,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultPlanProfileTe // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -758,9 +758,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADProcessManagerDefaultLVSPlanProfil // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -802,9 +802,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultPlanProfile // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADDTExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); @@ -846,9 +846,9 @@ TEST_F(TesseractProcessManagerUnit, RasterWAADDTProcessManagerDefaultLVSPlanProf // Define the program std::string freespace_profile = DEFAULT_PROFILE_KEY; - std::string approach_profile = "APPROACH"; - std::string process_profile = "PROCESS"; - std::string departure_profile = "DEPARTURE"; + std::string approach_profile = DEFAULT_PROFILE_KEY; + std::string process_profile = DEFAULT_PROFILE_KEY; + std::string departure_profile = DEFAULT_PROFILE_KEY; CompositeInstruction program = rasterWAADDTExampleProgram(freespace_profile, approach_profile, process_profile, departure_profile); From b10353fe1b03fdac06b028dd4975cf1d907ee557 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Jan 2022 19:49:14 -0600 Subject: [PATCH 45/46] Updated process manager examples to add default profiles and use only default profile names --- .../examples/freespace_manager_example.cpp | 1 + tesseract_process_managers/examples/memory_usage_example.cpp | 1 + tesseract_process_managers/examples/raster_manager_example.cpp | 3 ++- .../examples/taskflow_profiling_example.cpp | 1 + 4 files changed, 5 insertions(+), 1 deletion(-) diff --git a/tesseract_process_managers/examples/freespace_manager_example.cpp b/tesseract_process_managers/examples/freespace_manager_example.cpp index a169561fab2..bafcaf023a0 100644 --- a/tesseract_process_managers/examples/freespace_manager_example.cpp +++ b/tesseract_process_managers/examples/freespace_manager_example.cpp @@ -68,6 +68,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; diff --git a/tesseract_process_managers/examples/memory_usage_example.cpp b/tesseract_process_managers/examples/memory_usage_example.cpp index c090927d6d8..46e10ec123b 100644 --- a/tesseract_process_managers/examples/memory_usage_example.cpp +++ b/tesseract_process_managers/examples/memory_usage_example.cpp @@ -151,6 +151,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 5); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Solve process plan using Clock = std::chrono::high_resolution_clock; diff --git a/tesseract_process_managers/examples/raster_manager_example.cpp b/tesseract_process_managers/examples/raster_manager_example.cpp index 53eece8042d..32f68886a15 100644 --- a/tesseract_process_managers/examples/raster_manager_example.cpp +++ b/tesseract_process_managers/examples/raster_manager_example.cpp @@ -69,13 +69,14 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); // Create Process Planning Request ProcessPlanningRequest request; request.name = process_planner_names::RASTER_G_FT_PLANNER_NAME; // Define the program - CompositeInstruction program = rasterExampleProgram(); + CompositeInstruction program = rasterExampleProgram(DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY); request.instructions = Instruction(program); // Print Diagnostics diff --git a/tesseract_process_managers/examples/taskflow_profiling_example.cpp b/tesseract_process_managers/examples/taskflow_profiling_example.cpp index 97d20fecc45..c906f805ccc 100644 --- a/tesseract_process_managers/examples/taskflow_profiling_example.cpp +++ b/tesseract_process_managers/examples/taskflow_profiling_example.cpp @@ -99,6 +99,7 @@ int main() // Create Process Planning Server ProcessPlanningServer planning_server(std::make_shared(env), 1); planning_server.loadDefaultProcessPlanners(); + planning_server.loadDefaultProfiles(); for (int idx = -10; idx < 10; idx++) { From e7900e747188c028c67cd7186ab164d61557d83d Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sun, 9 Jan 2022 19:18:34 -0600 Subject: [PATCH 46/46] Clang tidy fixes --- .../ompl/profile/ompl_waypoint_profile.h | 2 +- .../ompl/src/ompl_motion_planner.cpp | 28 +++++++++---------- .../profile/ompl_composite_profile_rvss.cpp | 6 ++-- .../src/profile/ompl_waypoint_profile.cpp | 25 +++++++++-------- 4 files changed, 32 insertions(+), 29 deletions(-) diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h index ca8348368ca..73f56d768fb 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h @@ -8,7 +8,7 @@ namespace tesseract_planning class OMPLWaypointProfile : public WaypointProfile { public: - virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override; + std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override; private: friend class boost::serialization::access; diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index f2c072a94da..bd6cd12a92b 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -81,7 +81,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, } std::vector> createOMPLStates(const std::vector& joint_states, - ompl::base::SpaceInformationPtr si) + const ompl::base::SpaceInformationPtr& si) { std::vector> states; states.reserve(joint_states.size()); @@ -98,7 +98,7 @@ std::vector> createOMPLStates(const std::vector plan(ompl::geometric::SimpleSetupPtr simple_setup, +std::shared_ptr plan(const ompl::geometric::SimpleSetupPtr& simple_setup, const OMPLPlannerParameters& params, const unsigned n_output_states) { @@ -220,7 +220,7 @@ std::shared_ptr plan(ompl::geometric::SimpleSetupPtr si // space to be considered equivalent are not merged or connected. Also different planners add edges in different ways // and set different vertex/edge properties, so it may not make sense to combine them auto planner_data = std::make_shared(simple_setup->getSpaceInformation()); - for (auto planner : planners) + for (const ompl::base::PlannerPtr& planner : planners) planner->getPlannerData(*planner_data); // The planning data is actually owned by the planner instances themselves. Deep copy the planning information by @@ -240,7 +240,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra // In this case, we need to insert more states into the composite to cover the difference // Remember the composite does not include the start state, so compare its size with one less than the size of the // trajectory - if (seed.size() < trajectory.rows() - 1) + if (static_cast(seed.size()) < trajectory.rows() - 1) { const std::size_t diff = static_cast(trajectory.rows() - 1) - output.size(); output.insert(output.end(), diff, output.back()); @@ -259,7 +259,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra // Subsequent trajectory states go into the composite instruction // The index into the composite of these states is one less than the index of the trajectory state since the first // trajectory state was saved outside the composite - const std::size_t composite_idx = static_cast(i - 1); + const auto composite_idx = static_cast(i - 1); auto& move_instruction = output.at(composite_idx).as(); move_instruction.getWaypoint().as().position = trajectory.row(i); } @@ -342,7 +342,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ // Get the start waypoint profile and add the states to the SimpleSetup if (i == 0) { - const PlanInstruction& pi = request.instructions.getStartInstruction().as(); + const auto& pi = request.instructions.getStartInstruction().as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); @@ -351,8 +351,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ else { // Use the last state of the previous trajectory as the single start state for this plan - const MoveInstruction& mi = response.results.back().as().back().as(); - const StateWaypoint& sw = mi.getWaypoint().as(); + const auto& mi = response.results.back().as().back().as(); + const auto& sw = mi.getWaypoint().as(); start_states.push_back(sw.position); } @@ -365,12 +365,12 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ // Add the goal waypoint(s) { - const PlanInstruction& pi = request.instructions[i].as(); + const auto& pi = request.instructions[i].as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); - std::vector states = std::any_cast>(p->create(pi, *request.env)); + auto states = std::any_cast>(p->create(pi, *request.env)); auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation()); auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); @@ -428,14 +428,14 @@ bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request) { // Check that parameters are valid if (request.env == nullptr) - std::runtime_error("Environment is invalid (nullptr)"); + throw std::runtime_error("Environment is invalid (nullptr)"); if (request.instructions.empty()) - std::runtime_error("Request contains no instructions"); + throw std::runtime_error("Request contains no instructions"); if (request.instructions.size() != request.seed.size()) - std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) + - ") does not match seed size (" + std::to_string(request.seed.size()) + ")"); + throw std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) + + ") does not match seed size (" + std::to_string(request.seed.size()) + ")"); return true; } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index b3b2352ccce..3a4d7e5c321 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -21,7 +21,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio { for (const Instruction& inst : instruction) { - const PlanInstruction& goal_instruction = inst.as(); + const auto& goal_instruction = inst.as(); if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) throw std::runtime_error("Only freespace plan instruction types are currently supported"); } @@ -87,7 +87,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio break; } default: - CONSOLE_BRIDGE_logDebug("DOING NOTHING HERE"); + CONSOLE_BRIDGE_logDebug("No collision state validator added"); } simple_setup->setStateValidityChecker(csvc); } @@ -124,7 +124,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio break; } default: - CONSOLE_BRIDGE_logDebug("Nothing doing"); + CONSOLE_BRIDGE_logDebug("No collision validator added"); } simple_setup->getSpaceInformation()->setMotionValidator(mv); diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp index 034738854ca..ddbf416ba4b 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -5,7 +5,7 @@ namespace tesseract_planning { void checkCollision(const Eigen::VectorXd& state, const tesseract_environment::Environment& env, - tesseract_kinematics::JointGroup::ConstPtr manip) + const tesseract_kinematics::JointGroup::ConstPtr& manip) { tesseract_collision::DiscreteContactManager::Ptr contact_checker = env.getDiscreteContactManager(); tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state); @@ -28,7 +28,8 @@ void checkCollision(const Eigen::VectorXd& state, } } -Eigen::VectorXd updateLimits(Eigen::Ref joint_waypoint, tesseract_common::KinematicLimits limits) +Eigen::VectorXd updateLimits(const Eigen::Ref& joint_waypoint, + const tesseract_common::KinematicLimits& limits) { if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) throw std::runtime_error("State violates joint limits"); @@ -65,12 +66,12 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c tesseract_common::KinematicLimits limits = manip->getLimits(); tesseract_kinematics::IKSolutions valid_solutions; valid_solutions.reserve(joint_solutions.size()); - for (std::size_t i = 0; i < joint_solutions.size(); ++i) + for (const Eigen::VectorXd& js : joint_solutions) { try { // Update the joint solution based on the kinematic limits - Eigen::VectorXd solution = updateLimits(joint_solutions[i], limits); + Eigen::VectorXd solution = updateLimits(js, limits); // Ensure the solution is collision-free checkCollision(solution, env, manip); @@ -99,25 +100,27 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c std::any OMPLWaypointProfile::create(const Instruction& instruction, const tesseract_environment::Environment& env) const { - PlanInstruction plan_instruction = instruction.as(); - tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo(); + const auto& plan_instruction = instruction.as(); + const tesseract_common::ManipulatorInfo& mi = plan_instruction.getManipulatorInfo(); const Waypoint& waypoint = plan_instruction.getWaypoint(); if (isCartesianWaypoint(waypoint)) { - const CartesianWaypoint& cw = waypoint.as(); + const auto& cw = waypoint.as(); return getValidIKSolutions(cw, mi, env); } - else if (isJointWaypoint(waypoint)) + + if (isJointWaypoint(waypoint)) { - const JointWaypoint& jw = waypoint.as(); + const auto& jw = waypoint.as(); const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits()); checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); return std::vector{ updated_state }; } - else if (isStateWaypoint(waypoint)) + + if (isStateWaypoint(waypoint)) { - const StateWaypoint& sw = waypoint.as(); + const auto& sw = waypoint.as(); Eigen::Map state(sw.position.data(), sw.position.size()); const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits()); checkCollision(updated_state, env, env.getJointGroup(mi.manipulator));