From 88b812d0db9c15a6fa2bd902af4ee70827b83d35 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Oct 2022 10:20:30 -0500 Subject: [PATCH 01/17] Added structure for holding profile overrides --- .../include/tesseract_command_language/types.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tesseract_command_language/include/tesseract_command_language/types.h b/tesseract_command_language/include/tesseract_command_language/types.h index 1d362720db3..a253361ff06 100644 --- a/tesseract_command_language/include/tesseract_command_language/types.h +++ b/tesseract_command_language/include/tesseract_command_language/types.h @@ -31,6 +31,16 @@ namespace tesseract_planning { using ManipulatorInfo = tesseract_common::ManipulatorInfo; + +/** + * @brief Map that associates for override profile names (values) with specified task namespaces (keys) + * @details For example, a profile "default" might have the following override profiles names for various specific task + * namespaces + * - ["ompl", "custom_profile_1"] + * - ["time_parameterization", "custom_profile_2"] + */ +using ProfileOverrides = std::unordered_map; + } // namespace tesseract_planning #endif // TESSERACT_COMMAND_LANGUAGE_TYPES_H From 68d6186f083cf6214ca913de1507f4aaf5ff0758 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Oct 2022 10:22:06 -0500 Subject: [PATCH 02/17] Changed profile override structure in move and composite instruction to eliminate dependency on profile dictionary --- .../composite_instruction.h | 10 ++--- .../move_instruction.h | 18 ++++---- .../poly/move_instruction_poly.h | 42 +++++++++---------- .../src/composite_instruction.cpp | 12 ++++-- .../src/move_instruction.cpp | 24 ++++++++--- .../src/poly/move_instruction_poly.cpp | 17 ++++---- 6 files changed, 72 insertions(+), 51 deletions(-) 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 4c43af8001c..47aad9a627a 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include namespace tesseract_planning @@ -117,10 +117,10 @@ class CompositeInstruction void print(const std::string& prefix = "") const; void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; void setManipulatorInfo(tesseract_common::ManipulatorInfo info); const tesseract_common::ManipulatorInfo& getManipulatorInfo() const; @@ -386,7 +386,7 @@ class CompositeInstruction std::string profile_{ DEFAULT_PROFILE_KEY }; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr profile_overrides_; + ProfileOverrides profile_overrides_; /** @brief The order of the composite instruction */ CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED }; diff --git a/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract_command_language/include/tesseract_command_language/move_instruction.h index 430c9436ca8..7bf082632d1 100644 --- a/tesseract_command_language/include/tesseract_command_language/move_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/move_instruction.h @@ -35,7 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include namespace tesseract_planning @@ -139,16 +139,16 @@ class MoveInstruction tesseract_common::ManipulatorInfo& getManipulatorInfo(); void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; void setPathProfile(const std::string& profile); - const std::string& getPathProfile() const; + const std::string& getPathProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getPathProfileOverrides() const; const std::string& getDescription() const; @@ -183,10 +183,10 @@ class MoveInstruction std::string path_profile_; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr profile_overrides_; + ProfileOverrides profile_overrides_; /** @brief Dictionary of path profiles that will override named profiles for a specific task*/ - ProfileDictionary::ConstPtr path_profile_overrides_; + ProfileOverrides path_profile_overrides_; /** @brief The assigned waypoint (Cartesian, Joint or State) */ WaypointPoly waypoint_; diff --git a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h index de1d4c73331..fa5f54062fa 100644 --- a/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h +++ b/tesseract_command_language/include/tesseract_command_language/poly/move_instruction_poly.h @@ -40,7 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include #include #include #include @@ -131,11 +131,11 @@ struct MoveInstructionConcept // NOLINT const std::string& path_profile_const = c.getPathProfile(); UNUSED(path_profile_const); - c.setProfileOverrides(nullptr); + c.setProfileOverrides(ProfileOverrides{}); auto profile_overrides = c.getProfileOverrides(); UNUSED(profile_overrides); - c.setPathProfileOverrides(nullptr); + c.setPathProfileOverrides(ProfileOverrides{}); auto path_profile_overrides = c.getPathProfileOverrides(); UNUSED(path_profile_overrides); @@ -184,16 +184,16 @@ struct MoveInstructionInterface : tesseract_common::TypeErasureInterface virtual tesseract_common::ManipulatorInfo& getManipulatorInfo() = 0; virtual void setProfile(const std::string& profile) = 0; - virtual const std::string& getProfile() const = 0; + virtual const std::string& getProfile(const std::string& ns = "") const = 0; virtual void setPathProfile(const std::string& profile) = 0; - virtual const std::string& getPathProfile() const = 0; + virtual const std::string& getPathProfile(const std::string& ns = "") const = 0; - virtual void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0; - virtual ProfileDictionary::ConstPtr getProfileOverrides() const = 0; + virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual ProfileOverrides getProfileOverrides() const = 0; - virtual void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) = 0; - virtual ProfileDictionary::ConstPtr getPathProfileOverrides() const = 0; + virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual ProfileOverrides getPathProfileOverrides() const = 0; virtual void setMoveType(MoveInstructionType move_type) = 0; virtual MoveInstructionType getMoveType() const = 0; @@ -248,22 +248,22 @@ struct MoveInstructionInstance : tesseract_common::TypeErasureInstanceget().getManipulatorInfo(); } void setProfile(const std::string& profile) final { this->get().setProfile(profile); } - const std::string& getProfile() const final { return this->get().getProfile(); } + const std::string& getProfile(const std::string& ns = "") const final { return this->get().getProfile(ns); } void setPathProfile(const std::string& profile) final { this->get().setPathProfile(profile); } - const std::string& getPathProfile() const final { return this->get().getPathProfile(); } + const std::string& getPathProfile(const std::string& ns = "") const final { return this->get().getPathProfile(ns); } - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final + void setProfileOverrides(ProfileOverrides profile_overrides) final { this->get().setProfileOverrides(profile_overrides); } - ProfileDictionary::ConstPtr getProfileOverrides() const final { return this->get().getProfileOverrides(); } + ProfileOverrides getProfileOverrides() const final { return this->get().getProfileOverrides(); } - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) final + void setPathProfileOverrides(ProfileOverrides profile_overrides) final { this->get().setPathProfileOverrides(profile_overrides); } - ProfileDictionary::ConstPtr getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); } + ProfileOverrides getPathProfileOverrides() const final { return this->get().getPathProfileOverrides(); } void setMoveType(MoveInstructionType move_type) final { this->get().setMoveType(move_type); } MoveInstructionType getMoveType() const final { return this->get().getMoveType(); } @@ -314,16 +314,16 @@ struct MoveInstructionPoly : MoveInstructionPolyBase tesseract_common::ManipulatorInfo& getManipulatorInfo(); void setProfile(const std::string& profile); - const std::string& getProfile() const; + const std::string& getProfile(const std::string& ns = "") const; void setPathProfile(const std::string& profile); - const std::string& getPathProfile() const; + const std::string& getPathProfile(const std::string& ns = "") const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getPathProfileOverrides() const; void setMoveType(MoveInstructionType move_type); MoveInstructionType getMoveType() const; diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 47dc4153543..2f44e0b36ac 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -79,13 +79,19 @@ void CompositeInstruction::setProfile(const std::string& profile) { profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile; } -const std::string& CompositeInstruction::getProfile() const { return profile_; } +const std::string& CompositeInstruction::getProfile(const std::string& ns) const +{ + if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) + return profile_; + else + return profile_overrides_.at(ns); +} -void CompositeInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr CompositeInstruction::getProfileOverrides() const { return profile_overrides_; } +ProfileOverrides CompositeInstruction::getProfileOverrides() const { return profile_overrides_; } void CompositeInstruction::setManipulatorInfo(tesseract_common::ManipulatorInfo info) { diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp index b9eda05bee1..6a3a9d33b69 100644 --- a/tesseract_command_language/src/move_instruction.cpp +++ b/tesseract_command_language/src/move_instruction.cpp @@ -190,22 +190,34 @@ const tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() c tesseract_common::ManipulatorInfo& MoveInstruction::getManipulatorInfo() { return manipulator_info_; } void MoveInstruction::setProfile(const std::string& profile) { profile_ = profile; } -const std::string& MoveInstruction::getProfile() const { return profile_; } +const std::string& MoveInstruction::getProfile(const std::string& ns) const +{ + if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) + return profile_; + else + return profile_overrides_.at(ns); +} void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; } -const std::string& MoveInstruction::getPathProfile() const { return path_profile_; } +const std::string& MoveInstruction::getPathProfile(const std::string& ns) const +{ + if (ns.empty() || (path_profile_overrides_.find(ns) == path_profile_overrides_.end())) + return path_profile_; + else + return path_profile_overrides_.at(ns); +} -void MoveInstruction::setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr MoveInstruction::getProfileOverrides() const { return profile_overrides_; } +ProfileOverrides MoveInstruction::getProfileOverrides() const { return profile_overrides_; } -void MoveInstruction::setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides) +void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides) { path_profile_overrides_ = std::move(profile_overrides); } -ProfileDictionary::ConstPtr MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; } +ProfileOverrides MoveInstruction::getPathProfileOverrides() const { return path_profile_overrides_; } const std::string& MoveInstruction::getDescription() const { return description_; } diff --git a/tesseract_command_language/src/poly/move_instruction_poly.cpp b/tesseract_command_language/src/poly/move_instruction_poly.cpp index 9f3755c15d9..f2067670836 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -68,33 +68,36 @@ void tesseract_planning::MoveInstructionPoly::setProfile(const std::string& prof { getInterface().setProfile(profile); } -const std::string& tesseract_planning::MoveInstructionPoly::getProfile() const { return getInterface().getProfile(); } +const std::string& tesseract_planning::MoveInstructionPoly::getProfile(const std::string& ns) const +{ + return getInterface().getProfile(ns); +} void tesseract_planning::MoveInstructionPoly::setPathProfile(const std::string& profile) { getInterface().setPathProfile(profile); } -const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile() const +const std::string& tesseract_planning::MoveInstructionPoly::getPathProfile(const std::string& ns) const { - return getInterface().getPathProfile(); + return getInterface().getPathProfile(ns); } void tesseract_planning::MoveInstructionPoly::setProfileOverrides( - tesseract_planning::ProfileDictionary::ConstPtr profile_overrides) + tesseract_planning::ProfileOverrides profile_overrides) { getInterface().setProfileOverrides(std::move(profile_overrides)); } -tesseract_planning::ProfileDictionary::ConstPtr tesseract_planning::MoveInstructionPoly::getProfileOverrides() const +tesseract_planning::ProfileOverrides tesseract_planning::MoveInstructionPoly::getProfileOverrides() const { return getInterface().getProfileOverrides(); } void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides( - tesseract_planning::ProfileDictionary::ConstPtr profile_overrides) + tesseract_planning::ProfileOverrides profile_overrides) { getInterface().setPathProfileOverrides(std::move(profile_overrides)); } -tesseract_planning::ProfileDictionary::ConstPtr tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const +tesseract_planning::ProfileOverrides tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const { return getInterface().getPathProfileOverrides(); } From 338903faf596381d8aa6d93dcc491d1c900ff23b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Oct 2022 10:35:21 -0500 Subject: [PATCH 03/17] Remove remapping from plan request --- .../tesseract_motion_planners/core/types.h | 22 +------------------ 1 file changed, 1 insertion(+), 21 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h index 2d1934e2f65..46d08885c7d 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h @@ -30,18 +30,10 @@ #include #include #include +#include namespace tesseract_planning { -/** - * This used to store planner specific profile mapping with the request - * - * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles - * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to - * say Raster2 for the specific planner Descartes by Map>. - */ -using PlannerProfileRemapping = std::unordered_map>; - struct PlannerRequest { // LCOV_EXCL_START @@ -66,18 +58,6 @@ struct PlannerRequest */ CompositeInstruction instructions; - /** - * @brief This allows the remapping of the Plan Profile identified in the command language to a specific profile for a - * given motion planner. - */ - PlannerProfileRemapping plan_profile_remapping{}; - - /** - * @brief This allows the remapping of the Composite Profile identified in the command language to a specific profile - * for a given motion planner. - */ - PlannerProfileRemapping composite_profile_remapping{}; - /** @brief Indicate if output should be verbose */ bool verbose{ false }; From c1cef0c059ed9a2078e97c883201c47ae1209e96 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Oct 2022 10:36:40 -0500 Subject: [PATCH 04/17] Consolidate profile getter functions into profile dictionary --- .../profile_dictionary.h | 83 ++++++++++-------- .../tesseract_motion_planners/planner_utils.h | 87 ------------------- 2 files changed, 45 insertions(+), 125 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 9e04f04f581..86c8cf5ac44 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -35,19 +35,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -/** - * This used to store specific profile mapping with the request - * - * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles - * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to - * say Raster2 for the specific planner Descartes by Map>. - */ -using ProfileRemapping = std::unordered_map>; - /** * @brief This class is used to store profiles for motion planning and process planning * @details This is a thread safe class @@ -156,31 +149,6 @@ class ProfileDictionary } } - /** - * @brief Check if a profile exists - * @details If profile entry does not exist it also returns false - * @return True if profile exists, otherwise false - */ - template - bool hasProfile(const std::string& ns, const std::string& profile_name) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return false; - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - { - const auto& profile_map = - std::any_cast>&>(it2->second); - auto it3 = profile_map.find(profile_name); - if (it3 != profile_map.end()) - return true; - } - return false; - } - /** * @brief Get a profile by name * @details Check if the profile exist before calling this function, if missing an exception is thrown @@ -191,11 +159,50 @@ class ProfileDictionary std::shared_ptr getProfile(const std::string& ns, const std::string& profile_name) const { std::shared_lock lock(mutex_); - const auto& it = profiles_.at(ns); - const auto& it2 = it.at(std::type_index(typeid(ProfileType))); + + const std::unordered_map* it = nullptr; + try + { + it = &(profiles_.at(ns)); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "Failed to find entries for namespace '" << ns << "'. Available namespaces are: "; + for (auto it = profiles_.begin(); it != profiles_.end(); ++it) + { + ss << "'" << it->first << "', "; + } + std::throw_with_nested(std::runtime_error(ss.str())); + } + + const std::any* it2 = nullptr; + try + { + it2 = &(it->at(std::type_index(typeid(ProfileType)))); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "No entries for profile base class type '" << boost::core::demangle(typeid(ProfileType).name()) << "'"; + std::throw_with_nested(std::runtime_error(ss.str())); + } + const auto& profile_map = - std::any_cast>&>(it2); - return profile_map.at(profile_name); + std::any_cast>&>(*it2); + try + { + return profile_map.at(profile_name); + } + catch (const std::exception&) + { + std::stringstream ss; + ss << "No entries for profile '" << profile_name << "' in namespace '" << ns << "'. Available profiles are: "; + for (auto it = profile_map.begin(); it != profile_map.end(); ++it) + ss << "'" << it->first << "', "; + + std::throw_with_nested(std::runtime_error(ss.str())); + } } /** diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h index 0ff12e5a1a4..88aaee9def4 100644 --- a/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h +++ b/tesseract_motion_planners/core/include/tesseract_motion_planners/planner_utils.h @@ -92,93 +92,6 @@ bool isValidState(const tesseract_kinematics::ForwardKinematics::ConstPtr& robot return !(robot_config != RobotConfig::FUT && robot_config != RobotConfig::NUT); } -/** - * @brief Get the profile string taking into account defaults and profile remapping - * @param ns Used to look up if there are remappings available - * @param profile The requested profile name in the instructions - * @param profile_remapping Remapping used to remap a profile name based on the planner name - * @param default_profile Default = DEFAULT. This is set if profile.empty() - * @return The profile string taking into account defaults and profile remapping - */ -inline std::string getProfileString(const std::string& ns, - const std::string& profile, - const PlannerProfileRemapping& profile_remapping, - std::string default_profile = DEFAULT_PROFILE_KEY) -{ - std::string results = profile; - if (profile.empty()) - results = std::move(default_profile); - - // Check for remapping of profile - auto remap = profile_remapping.find(ns); - if (remap != profile_remapping.end()) - { - auto p = remap->second.find(profile); - if (p != remap->second.end()) - results = p->second; - } - return results; -} - -/** - * @brief Gets the profile specified from the profile map - * @param ns The namespace to search for requested profile - * @param profile The requested profile - * @param profile_map map that contains the profiles - * @param default_profile Profile that is returned if the requested profile is not found in the map. Default = nullptr - * @return The profile requested if found. Otherwise the default_profile - */ -template -std::shared_ptr getProfile(const std::string& ns, - const std::string& profile, - const ProfileDictionary& profile_dictionary, - std::shared_ptr default_profile = nullptr) -{ - if (profile_dictionary.hasProfile(ns, profile)) - return profile_dictionary.getProfile(ns, profile); - - CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. " - "Available " - "profiles:", - profile.c_str(), - ns.c_str(), - typeid(ProfileType).name()); - - if (profile_dictionary.hasProfileEntry(ns)) - { - for (const auto& pair : profile_dictionary.getProfileEntry(ns)) - { - CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str()); - } - } - - return default_profile; -} - -/** - * @brief Returns either nominal_profile or override based on task name passed in - * @param ns The namespace to search for requested profile - * @param profile The name used to look up if there is a profile override - * @param nominal_profile Profile that will be used if no override is present - * @param overrides Dictionary of profile overrides that will override the nominal profile if present for this task. - * Default = nullptr - * @return The nominal_profile or override based on the task name passed in - */ -template -std::shared_ptr applyProfileOverrides(const std::string& ns, - const std::string& profile, - const std::shared_ptr& nominal_profile, - const ProfileDictionary::ConstPtr& overrides = nullptr) -{ - if (!overrides) - return nominal_profile; - - if (overrides->hasProfile(ns, profile)) - return overrides->getProfile(ns, profile); - - return nominal_profile; -} - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_PLANNER_UTILS_H From 563b981001fb1cc1f3b18a763b889f9993914749 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Sat, 8 Oct 2022 10:37:13 -0500 Subject: [PATCH 05/17] Update planners to use profile dictionary to get profiles --- .../impl/descartes_motion_planner.hpp | 10 ++---- .../ompl/src/ompl_motion_planner.cpp | 8 +---- .../simple/src/simple_motion_planner.cpp | 13 +++---- .../trajopt/src/trajopt_motion_planner.cpp | 29 ++++------------ .../src/trajopt_ifopt_motion_planner.cpp | 34 ++++++------------- 5 files changed, 23 insertions(+), 71 deletions(-) diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp index 7743e5b5839..b3f462674ad 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp @@ -283,14 +283,8 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) throw std::runtime_error("Descartes, working_frame is empty!"); // 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("DescartesMotionPlanner: Invalid profile"); + auto cur_plan_profile = + request.profiles->getProfile>(name_, plan_instruction.getProfile(name_)); if (plan_instruction.getWaypoint().isCartesianWaypoint()) { diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 74dc5539c31..47d254ec35d 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -321,13 +321,7 @@ OMPLProblemConfig OMPLMotionPlanner::createSubProblem(const PlannerRequest& requ std::vector active_link_names = manip->getActiveLinkNames(); // Get Plan Profile - std::string profile = end_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, end_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); + auto cur_plan_profile = request.profiles->getProfile(name_, end_instruction.getProfile(name_)); /** @todo Should check that the joint names match the order of the manipulator */ OMPLProblemConfig config; diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index cff100349d1..2ce6703dbef 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -190,18 +190,13 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp SimplePlannerPlanProfile::ConstPtr plan_profile; if (base_instruction.getPathProfile().empty()) { - std::string profile = getProfileString(name_, base_instruction.getProfile(), request.plan_profile_remapping); - plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + plan_profile = + request.profiles->getProfile(name_, base_instruction.getProfile(name_)); } else { - std::string profile = - getProfileString(name_, base_instruction.getPathProfile(), request.plan_profile_remapping); - plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - plan_profile = applyProfileOverrides(name_, profile, plan_profile, base_instruction.getProfileOverrides()); + plan_profile = + request.profiles->getProfile(name_, base_instruction.getPathProfile(name_)); } if (!plan_profile) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 13905c9d6d0..71258c99aa3 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -210,15 +210,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const } // Apply Solver parameters - std::string profile = request.instructions.getProfile(); - ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, request.plan_profile_remapping); - TrajOptSolverProfile::ConstPtr solver_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); - if (!solver_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - + TrajOptSolverProfile::ConstPtr solver_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); solver_profile->apply(*pci); // Get kinematics information @@ -250,12 +243,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); + TrajOptPlanProfile::ConstPtr cur_plan_profile = + request.profiles->getProfile(name_, move_instruction.getProfile(name_)); if (move_instruction.getWaypoint().isCartesianWaypoint()) { @@ -333,14 +322,8 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const for (long i = 0; i < pci->basic_info.n_steps; ++i) pci->init_info.data.row(i) = seed_states[static_cast(i)]; - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - TrajOptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); - if (!cur_composite_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); - + TrajOptCompositeProfile::ConstPtr cur_composite_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); cur_composite_profile->apply(*pci, 0, pci->basic_info.n_steps - 1, composite_mi, active_links, fixed_steps); return pci; diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 42b0e7f859d..9d2e3b6537f 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -215,16 +215,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co } // Apply Solver parameters - std::string profile = request.instructions.getProfile(); - ProfileDictionary::ConstPtr profile_overrides = request.instructions.getProfileOverrides(); - profile = getProfileString(name_, profile, PlannerProfileRemapping()); - // TrajOptSolverProfile::ConstPtr solver_profile = - // getProfile(profile, solver_profiles, std::make_shared()); - // solver_profile = applyProfileOverrides(name, solver_profile, profile_overrides); - // if (!solver_profile) - // throw std::runtime_error("TrajOptSolverConfig: Invalid profile"); - - // solver_profile->apply(*pci); + // TrajOptSolverProfile::ConstPtr solver_profile = request.profiles->getProfile(name_, + // request.instructions.getProfile(name_)); solver_profile->apply(*pci); // Get kinematics information tesseract_environment::Environment::ConstPtr env = request.env; @@ -255,12 +247,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co throw std::runtime_error("TrajOpt, working_frame is empty!"); // Get Plan Profile - std::string profile = getProfileString(name_, move_instruction.getProfile(), request.plan_profile_remapping); - TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_plan_profile = applyProfileOverrides(name_, profile, cur_plan_profile, move_instruction.getProfileOverrides()); - if (!cur_plan_profile) - throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); + TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = + request.profiles->getProfile(name_, move_instruction.getProfile(name_)); if (move_instruction.getWaypoint().isCartesianWaypoint()) { @@ -313,7 +301,10 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co fixed_steps.push_back(i); } } - else if (move_instruction.getWaypoint().isStateWaypoint()) + else if (move_instruction.getWaypoint().isStateWaypoint()) // solver_profile = applyProfileOverrides(name, + // solver_profile, profile_overrides); + // if (!solver_profile) + // throw std::runtime_error("TrajOptSolverConfig: Invalid profile"); { assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); const auto& swp = move_instruction.getWaypoint().as(); @@ -343,13 +334,8 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // ---------------- // Translate TCL for CompositeInstructions // ---------------- - profile = getProfileString(name_, request.instructions.getProfile(), request.composite_profile_remapping); - TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - cur_composite_profile = - applyProfileOverrides(name_, profile, cur_composite_profile, request.instructions.getProfileOverrides()); - if (!cur_composite_profile) - throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); + TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = + request.profiles->getProfile(name_, request.instructions.getProfile(name_)); cur_composite_profile->apply( *problem, 0, static_cast(problem->vars.size()) - 1, composite_mi, active_links, fixed_steps); From 2d65286bdc8aa13d6cf888ff7608bcbfe0c55a42 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 15:10:13 -0500 Subject: [PATCH 06/17] xAdded unit test for profile overrides --- .../test/command_language_unit.cpp | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/tesseract_command_language/test/command_language_unit.cpp b/tesseract_command_language/test/command_language_unit.cpp index 78a9e3715c5..2727340e04c 100644 --- a/tesseract_command_language/test/command_language_unit.cpp +++ b/tesseract_command_language/test/command_language_unit.cpp @@ -673,6 +673,39 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT } } +TEST(TesseractCommandLanguageMoveInstructionPolyUnit, ProfileOverrides) +{ + Eigen::VectorXd jv = Eigen::VectorXd::Ones(6); + std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; + StateWaypointPoly swp(StateWaypoint(jn, jv)); + MoveInstruction instr(swp, MoveInstructionType::START, DEFAULT_PROFILE_KEY, DEFAULT_PROFILE_KEY); + + // Create arbitrary profile overrides under arbitrary namespaces + const std::string ns1 = "ns1"; + const std::string ns1_profile = "profile1"; + const std::string ns2 = "ns2"; + const std::string ns2_profile = "profile2"; + { + ProfileOverrides overrides; + overrides[ns1] = ns1_profile; + overrides[ns2] = ns2_profile; + instr.setProfileOverrides(overrides); + instr.setPathProfileOverrides(overrides); + } + + // Profile Overrides + EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); + EXPECT_EQ(instr.getProfile(ns1), ns1_profile); + EXPECT_EQ(instr.getProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getProfile("nonexistent_ns"), DEFAULT_PROFILE_KEY); + + // Path Profile Overrides + EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); + EXPECT_EQ(instr.getPathProfile(ns1), ns1_profile); + EXPECT_EQ(instr.getPathProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getPathProfile("nonexistent_ns"), DEFAULT_PROFILE_KEY); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From a6fb1514f63552e56c4b2f669bf2ccbba15f50a7 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 15:13:29 -0500 Subject: [PATCH 07/17] Updated profile dictionary unit tests --- .../test/profile_dictionary_tests.cpp | 46 ++++++++++--------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 5159c79055e..093ee2ae24e 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -32,6 +32,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP struct ProfileBase { + using ConstPtr = std::shared_ptr; int a{ 0 }; }; @@ -43,6 +44,7 @@ struct ProfileTest : public ProfileBase struct ProfileBase2 { + using ConstPtr = std::shared_ptr; int b{ 0 }; }; @@ -58,30 +60,28 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_FALSE(profiles.hasProfileEntry("ns")); + EXPECT_THROW(profiles.getProfile("ns", "key"), std::runtime_error); profiles.addProfile("ns", "key", std::make_shared()); - EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); + ProfileBase::ConstPtr profile; + ASSERT_NO_THROW(profile = profiles.getProfile("ns", "key")); - auto profile = profiles.getProfile("ns", "key"); - - EXPECT_TRUE(profile != nullptr); + ASSERT_TRUE(profile != nullptr); EXPECT_EQ(profile->a, 0); // Check add same profile with different key profiles.addProfile("ns", "key2", profile); - EXPECT_TRUE(profiles.hasProfile("ns", "key2")); - auto profile2 = profiles.getProfile("ns", "key2"); - EXPECT_TRUE(profile2 != nullptr); + ProfileBase::ConstPtr profile2; + ASSERT_NO_THROW(profile2 = profiles.getProfile("ns", "key2")); + ASSERT_TRUE(profile2 != nullptr); EXPECT_EQ(profile2->a, 0); // Check replacing a profile profiles.addProfile("ns", "key", std::make_shared(10)); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); - auto profile_check = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check != nullptr); + ProfileBase::ConstPtr profile_check; + ASSERT_NO_THROW(profile_check = profiles.getProfile("ns", "key")); + ASSERT_TRUE(profile_check != nullptr); EXPECT_EQ(profile_check->a, 10); auto profile_map = profiles.getProfileEntry("ns"); @@ -90,12 +90,13 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT EXPECT_EQ(it->second->a, 10); profiles.addProfile("ns", "key", std::make_shared(20)); - auto profile_check2 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check2 != nullptr); + ProfileBase::ConstPtr profile_check2; + ASSERT_NO_THROW(profile_check2 = profiles.getProfile("ns", "key")); + ASSERT_TRUE(profile_check2 != nullptr); EXPECT_EQ(profile_check2->a, 20); // Request a profile entry namespace that does not exist - EXPECT_ANY_THROW(profiles.getProfileEntry("DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "DoesNotExist")); // NOLINT // Request a profile that does not exist EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "key")); // NOLINT @@ -104,10 +105,10 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT EXPECT_ANY_THROW(profiles.getProfile("ns", "DoesNotExist")); // NOLINT // Check adding a empty namespace - EXPECT_ANY_THROW(profiles.addProfile("", "key3", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("", "key3", nullptr)); // NOLINT // Check adding a empty key - EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "", nullptr)); // NOLINT // Check adding a nullptr profile EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // NOLINT @@ -115,13 +116,14 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT // Add different profile entry profiles.addProfile("ns", "key", std::make_shared(5)); EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); - auto profile_check3 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check3 != nullptr); + ProfileBase2::ConstPtr profile_check3; + ASSERT_NO_THROW(profile_check3 = profiles.getProfile("ns", "key")); + ASSERT_TRUE(profile_check3 != nullptr); EXPECT_EQ(profile_check3->b, 5); // Check that other profile entry with same key is not affected - auto profile_check4 = profiles.getProfile("ns", "key"); - EXPECT_TRUE(profile_check4 != nullptr); + ProfileBase::ConstPtr profile_check4; + ASSERT_NO_THROW(profile_check4 = profiles.getProfile("ns", "key")); + ASSERT_TRUE(profile_check4 != nullptr); EXPECT_EQ(profile_check4->a, 20); } From 5d8c4af9ccc6425bc12f9e7eeaca57beb7458231 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 15:39:51 -0500 Subject: [PATCH 08/17] Updated Trajopt planner test to add solver profile --- .../test/trajopt_planner_tests.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/tesseract_motion_planners/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/test/trajopt_planner_tests.cpp index cf2ccc18e7b..a0f350dcc15 100644 --- a/tesseract_motion_planners/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/test/trajopt_planner_tests.cpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -136,11 +137,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -211,11 +214,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -291,11 +296,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -375,11 +382,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -459,11 +468,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -543,11 +554,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -625,11 +638,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -687,11 +702,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT plan_profile->term_type = trajopt::TermType::TT_COST; // Everything associated with profile is now added as a cost auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); From a27a2d6f73ace2280e964136d687bba9c604135c Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:03:38 -0500 Subject: [PATCH 09/17] Remove obsolete overrides object from task composer problem --- .../planning/planning_task_composer_problem.h | 27 --------------- .../src/planning_task_composer_problem.cpp | 34 ------------------- 2 files changed, 61 deletions(-) diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h index 43331ca2c93..f53e70afe3f 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/planning_task_composer_problem.h @@ -56,21 +56,6 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"); - PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - tesseract_common::ManipulatorInfo manip_info, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles = nullptr, - std::string name = "unset"); - - PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles = nullptr, - std::string name = "unset"); - PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles = nullptr, @@ -91,18 +76,6 @@ struct PlanningTaskComposerProblem : public TaskComposerProblem /** @brief The Profiles to use */ ProfileDictionary::ConstPtr profiles; - /** - * @brief This allows the remapping of the Move Profile identified in the command language to a specific profile for a - * given motion planner. - */ - ProfileRemapping move_profile_remapping; - - /** - * @brief This allows the remapping of the Composite Profile identified in the command language to a specific profile - * for a given motion planner. - */ - ProfileRemapping composite_profile_remapping; - TaskComposerProblem::UPtr clone() const override; bool operator==(const PlanningTaskComposerProblem& rhs) const; diff --git a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp index bc7eeec122b..4eb75cc65fc 100644 --- a/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp +++ b/tesseract_task_composer/planning/src/planning_task_composer_problem.cpp @@ -58,36 +58,6 @@ PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment:: { } -PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - tesseract_common::ManipulatorInfo manip_info, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles, - std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)) - , env(std::move(env)) - , manip_info(std::move(manip_info)) - , profiles(std::move(profiles)) - , move_profile_remapping(std::move(move_profile_remapping)) - , composite_profile_remapping(std::move(composite_profile_remapping)) -{ -} - -PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, - ProfileRemapping move_profile_remapping, - ProfileRemapping composite_profile_remapping, - TaskComposerDataStorage input_data, - ProfileDictionary::ConstPtr profiles, - std::string name) - : TaskComposerProblem(std::move(input_data), std::move(name)) - , env(std::move(env)) - , profiles(std::move(profiles)) - , move_profile_remapping(std::move(move_profile_remapping)) - , composite_profile_remapping(std::move(composite_profile_remapping)) -{ -} - PlanningTaskComposerProblem::PlanningTaskComposerProblem(tesseract_environment::Environment::ConstPtr env, TaskComposerDataStorage input_data, ProfileDictionary::ConstPtr profiles, @@ -108,8 +78,6 @@ bool PlanningTaskComposerProblem::operator==(const PlanningTaskComposerProblem& equal &= tesseract_common::pointersEqual(env, rhs.env); equal &= manip_info == rhs.manip_info; // equal &= tesseract_common::pointersEqual(profiles, rhs.profiles); - equal &= move_profile_remapping == rhs.move_profile_remapping; - equal &= composite_profile_remapping == rhs.composite_profile_remapping; return equal; } @@ -123,8 +91,6 @@ void PlanningTaskComposerProblem::serialize(Archive& ar, const unsigned int /*ve ar& boost::serialization::make_nvp("manip_info", manip_info); /** @todo Fix after profiles are serializable */ // ar& boost::serialization::make_nvp("profiles", profiles); - ar& boost::serialization::make_nvp("move_profile_remapping", move_profile_remapping); - ar& boost::serialization::make_nvp("composite_profile_remapping", composite_profile_remapping); } } // namespace tesseract_planning From ba495e06d5e61e0c15955be3d25054a6eb74d609 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:05:01 -0500 Subject: [PATCH 10/17] Updated nodes to get profiles with new API --- .../planning/nodes/motion_planner_task.hpp | 2 -- .../planning/src/nodes/check_input_task.cpp | 21 +++++++++++-------- .../nodes/continuous_contact_check_task.cpp | 7 +------ .../src/nodes/discrete_contact_check_task.cpp | 6 +----- .../src/nodes/fix_state_bounds_task.cpp | 6 +----- .../src/nodes/fix_state_collision_task.cpp | 6 +----- ...iterative_spline_parameterization_task.cpp | 21 +++++++------------ .../planning/src/nodes/min_length_task.cpp | 6 +----- .../src/nodes/profile_switch_task.cpp | 6 +----- .../ruckig_trajectory_smoothing_task.cpp | 21 +++++++------------ .../time_optimal_parameterization_task.cpp | 7 ++----- .../src/nodes/upsample_trajectory_task.cpp | 6 +----- 12 files changed, 37 insertions(+), 78 deletions(-) diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp index 0fd17557891..a9c03d26fb0 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/motion_planner_task.hpp @@ -146,8 +146,6 @@ class MotionPlannerTask : public TaskComposerTask request.env = problem.env; request.instructions = instructions; request.profiles = problem.profiles; - request.plan_profile_remapping = problem.move_profile_remapping; - request.composite_profile_remapping = problem.composite_profile_remapping; request.format_result_as_input = format_result_as_input_; // -------------------- diff --git a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp index 9fa7e121ca0..a5ac5949d9d 100644 --- a/tesseract_task_composer/planning/src/nodes/check_input_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/check_input_task.cpp @@ -80,16 +80,19 @@ TaskComposerNodeInfo::UPtr CheckInputTask::runImpl(TaskComposerInput& input, return info; } - const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); - - if (!cur_composite_profile->isValid(input)) + try + { + const auto& ci = input_data_poly.as(); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); + if (!cur_composite_profile->isValid(input)) + { + info->message = "Validator failed"; + return info; + } + } + catch (const std::exception& ex) { - info->message = "Validator failed"; + info->message = ex.what(); return info; } } diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index 92c0357e628..f3f60d00a4e 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -90,12 +90,7 @@ TaskComposerNodeInfo::UPtr ContinuousContactCheckTask::runImpl(TaskComposerInput // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto default_profile = std::make_shared(); - default_profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - auto cur_composite_profile = getProfile(name_, profile, *problem.profiles, default_profile); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(problem.manip_info); diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index 33ccc29ccd1..1ba12ff4e88 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -89,11 +89,7 @@ TaskComposerNodeInfo::UPtr DiscreteContactCheckTask::runImpl(TaskComposerInput& // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(problem.manip_info); diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp index c75ecdf859f..f52679156ee 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_bounds_task.cpp @@ -96,11 +96,7 @@ TaskComposerNodeInfo::UPtr FixStateBoundsTask::runImpl(TaskComposerInput& input, auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); limits.joint_limits.col(0) = limits.joint_limits.col(0).array() + cur_composite_profile->lower_bounds_reduction; limits.joint_limits.col(1) = limits.joint_limits.col(1).array() - cur_composite_profile->upper_bounds_reduction; diff --git a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp index 7658488d0a6..75f56354918 100644 --- a/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/fix_state_collision_task.cpp @@ -366,11 +366,7 @@ TaskComposerNodeInfo::UPtr FixStateCollisionTask::runImpl(TaskComposerInput& inp auto& ci = input_data_poly.as(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); switch (cur_composite_profile->mode) { diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index 9aaed735dda..0bf6b85e28e 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -114,11 +114,8 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + problem.profiles->getProfile(name_, ci.getProfile(name_)); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); @@ -145,20 +142,18 @@ TaskComposerNodeInfo::UPtr IterativeSplineParameterizationTask::runImpl(TaskComp for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) { const auto& mi = flattened[static_cast(idx)].get().as(); - std::string move_profile = mi.getProfile(); - - // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, problem.move_profile_remapping); - auto cur_move_profile = getProfile( - name_, move_profile, *problem.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters - if (cur_move_profile) + try { + auto cur_move_profile = + input.profiles->getProfile(name_, mi.getProfile(name_)); velocity_scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; acceleration_scaling_factors[idx] = cur_move_profile->max_acceleration_scaling_factor; } + catch (const std::exception&) + { + } } // Solve using parameters diff --git a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 732999d6d31..30f434ceaba 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -90,11 +90,7 @@ TaskComposerNodeInfo::UPtr MinLengthTask::runImpl(TaskComposerInput& input, // Get Composite Profile const auto& ci = input_data_poly.as(); long cnt = ci.getMoveInstructionCount(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); if (cnt < cur_composite_profile->min_length) { diff --git a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp index 280dfebf8e5..b8922aa443c 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -82,11 +82,7 @@ TaskComposerNodeInfo::UPtr ProfileSwitchTask::runImpl(TaskComposerInput& input, // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = - getProfile(name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); // Return the value specified in the profile CONSOLE_BRIDGE_logDebug("ProfileSwitchProfile returning %d", cur_composite_profile->return_value); diff --git a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp index dc07ddf0f08..3684b50cbcd 100644 --- a/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/ruckig_trajectory_smoothing_task.cpp @@ -99,11 +99,8 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + problem.profiles->getProfile(name_, ci.getProfile(name_)); RuckigTrajectorySmoothing solver(cur_composite_profile->duration_extension_fraction, cur_composite_profile->max_duration_extension_factor); @@ -135,21 +132,19 @@ TaskComposerNodeInfo::UPtr RuckigTrajectorySmoothingTask::runImpl(TaskComposerIn for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) { const auto& mi = flattened[static_cast(idx)].get().as(); - std::string move_profile = mi.getProfile(); - - // Check for remapping of the plan profile - move_profile = getProfileString(name_, profile, problem.move_profile_remapping); - auto cur_move_profile = getProfile( - name_, move_profile, *problem.profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(name_, profile, cur_move_profile, mi.profile_overrides); // If there is a move profile associated with it, override the parameters - if (cur_move_profile) + try { + auto cur_move_profile = + input.profiles->getProfile(name_, mi.getProfile(name_)); velocity_scaling_factors[idx] = cur_move_profile->max_velocity_scaling_factor; acceleration_scaling_factors[idx] = cur_move_profile->max_acceleration_scaling_factor; jerk_scaling_factors[idx] = cur_move_profile->max_jerk_scaling_factor; } + catch (const std::exception&) + { + } } // Solve using parameters diff --git a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp index c6a75765358..e852cdc05fd 100644 --- a/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/time_optimal_parameterization_task.cpp @@ -105,11 +105,8 @@ TaskComposerNodeInfo::UPtr TimeOptimalParameterizationTask::runImpl(TaskComposer auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = + problem.profiles->getProfile(name_, ci.getProfile(name_)); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); diff --git a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp index 289edb7c764..4fe84fefbb9 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -89,11 +89,7 @@ TaskComposerNodeInfo::UPtr UpsampleTrajectoryTask::runImpl(TaskComposerInput& in // Get Composite Profile const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(name_, profile, problem.composite_profile_remapping); - auto cur_composite_profile = getProfile( - name_, profile, *problem.profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(name_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = problem.profiles->getProfile(name_, ci.getProfile(name_)); assert(cur_composite_profile->longest_valid_segment_length > 0); InstructionPoly start_instruction; From 815a631dbf4664cb3cea052e9e7003964bd2603a Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:06:35 -0500 Subject: [PATCH 11/17] Updated task composer unit test --- .../test/tesseract_task_composer_unit.cpp | 81 ++++++++++--------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp index 829cb03fbba..d5aa81ef06a 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_unit.cpp @@ -11,17 +11,19 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include + #include #include #include #include +#include #include -#include #include #include #include #include +#include #include #include @@ -302,12 +304,13 @@ TEST_F(TesseractTaskComposerUnit, RasterProcessManagerDefaultPlanProfileTest) / CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -345,12 +348,13 @@ TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultPlanProfileTe CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -388,12 +392,13 @@ TEST_F(TesseractTaskComposerUnit, RasterGlobalProcessManagerDefaultLVSPlanProfil CompositeInstruction program = rasterExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -431,12 +436,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultPlanProfileTest CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -474,12 +480,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyProcessManagerDefaultLVSPlanProfileT CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -517,12 +524,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultPlanProfi CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; @@ -560,12 +568,13 @@ TEST_F(TesseractTaskComposerUnit, RasterOnlyGlobalProcessManagerDefaultLVSPlanPr CompositeInstruction program = rasterOnlyExampleProgram(freespace_profile, process_profile); // Add profiles to planning server - auto default_simple_plan_profile = std::make_shared(); auto profiles = std::make_shared(); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, freespace_profile, default_simple_plan_profile); - profiles->addProfile( - SIMPLE_DEFAULT_NAMESPACE, process_profile, default_simple_plan_profile); + addDefaultPlannerProfiles(*profiles, { freespace_profile, process_profile }); + addDefaultTaskComposerProfiles(*profiles, { freespace_profile, process_profile }); + + auto lvs_simple_plan_profile = std::make_shared(); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, freespace_profile, lvs_simple_plan_profile); + profiles->addProfile(SIMPLE_DEFAULT_NAMESPACE, process_profile, lvs_simple_plan_profile); // Create data storage TaskComposerDataStorage task_data; From d0749c363942d8b366f818ad5ec405ff78693f57 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:06:56 -0500 Subject: [PATCH 12/17] Updated task composer examples --- .../examples/task_composer_raster_example.cpp | 2 ++ .../examples/task_composer_trajopt_example.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index 96739478343..eddc66f1e4a 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -51,6 +51,8 @@ int main() // Define profiles auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); // Define the program CompositeInstruction program = test_suite::rasterExampleProgram(); diff --git a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp index 90b24bf1232..82d62ba318b 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -52,6 +52,8 @@ int main() // Define profiles auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); // Define the program CompositeInstruction program = test_suite::freespaceExampleProgramIIWA(); From ddb9cd4f61362927e2f5496e39f710af1ed8a2f1 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:51:25 -0500 Subject: [PATCH 13/17] Updated profiles in motion planner examples --- .../examples/chain_example.cpp | 3 ++ .../examples/freespace_example.cpp | 3 ++ .../examples/raster_example.cpp | 35 +++++++++++-------- 3 files changed, 26 insertions(+), 15 deletions(-) diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index 5300c6df60f..60dd665c9aa 100644 --- a/tesseract_motion_planners/examples/chain_example.cpp +++ b/tesseract_motion_planners/examples/chain_example.cpp @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -143,6 +144,7 @@ int main(int /*argc*/, char** /*argv*/) auto descartes_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); @@ -152,6 +154,7 @@ int main(int /*argc*/, char** /*argv*/) profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "DEFAULT", descartes_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/examples/freespace_example.cpp b/tesseract_motion_planners/examples/freespace_example.cpp index b53cd9acb86..42291571215 100644 --- a/tesseract_motion_planners/examples/freespace_example.cpp +++ b/tesseract_motion_planners/examples/freespace_example.cpp @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -114,6 +115,7 @@ int main(int /*argc*/, char** /*argv*/) auto ompl_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); @@ -123,6 +125,7 @@ int main(int /*argc*/, char** /*argv*/) 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(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create Planning Request PlannerRequest request; diff --git a/tesseract_motion_planners/examples/raster_example.cpp b/tesseract_motion_planners/examples/raster_example.cpp index 7f054a7d894..bb6efd9421f 100644 --- a/tesseract_motion_planners/examples/raster_example.cpp +++ b/tesseract_motion_planners/examples/raster_example.cpp @@ -43,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include @@ -90,7 +91,7 @@ int main(int /*argc*/, char** /*argv*/) auto kin_group = env->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); auto cur_state = env->getState(); - CompositeInstruction program("raster_program", CompositeInstructionOrder::ORDERED, manip); + CompositeInstruction program(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, manip); // Start Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(kin_group->getJointNames(), Eigen::VectorXd::Zero(6)) }; @@ -114,14 +115,14 @@ int main(int /*argc*/, char** /*argv*/) Eigen::Quaterniond(0, 0, -1.0, 0)); // Define raster move instruction - MoveInstruction plan_c0(wp2, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c1(wp3, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c2(wp4, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c3(wp5, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c4(wp6, MoveInstructionType::LINEAR, "RASTER"); - MoveInstruction plan_c5(wp7, MoveInstructionType::LINEAR, "RASTER"); - - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_c0(wp2, MoveInstructionType::LINEAR); + MoveInstruction plan_c1(wp3, MoveInstructionType::LINEAR); + MoveInstruction plan_c2(wp4, MoveInstructionType::LINEAR); + MoveInstruction plan_c3(wp5, MoveInstructionType::LINEAR); + MoveInstruction plan_c4(wp6, MoveInstructionType::LINEAR); + MoveInstruction plan_c5(wp7, MoveInstructionType::LINEAR); + + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("from_start_plan"); CompositeInstruction from_start; from_start.setDescription("from_start"); @@ -142,7 +143,7 @@ int main(int /*argc*/, char** /*argv*/) } { - MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE); plan_f1.setDescription("transition_from_end_plan"); CompositeInstruction transition_from_end; transition_from_end.setDescription("transition_from_end"); @@ -171,7 +172,7 @@ int main(int /*argc*/, char** /*argv*/) } { - MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f1(wp1, MoveInstructionType::FREESPACE); plan_f1.setDescription("transition_from_end_plan"); CompositeInstruction transition_from_end; transition_from_end.setDescription("transition_from_end"); @@ -199,7 +200,7 @@ int main(int /*argc*/, char** /*argv*/) program.push_back(raster_segment); } - MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE, "freespace_profile"); + MoveInstruction plan_f2(wp1, MoveInstructionType::FREESPACE); plan_f2.setDescription("to_end_plan"); CompositeInstruction to_end; to_end.setDescription("to_end"); @@ -216,15 +217,19 @@ int main(int /*argc*/, char** /*argv*/) auto descartes_plan_profile = std::make_shared(); auto trajopt_plan_profile = std::make_shared(); auto trajopt_composite_profile = std::make_shared(); + auto trajopt_solver_profile = std::make_shared(); // Create a interpolated program CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "DEFAULT", descartes_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile>( + DESCARTES_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, descartes_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_plan_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_solver_profile); // Create Planning Request PlannerRequest request; From 7ca3918fdddacc125de5071c53042f6120a3c345 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 10 Oct 2022 20:51:50 -0500 Subject: [PATCH 14/17] Updated profiles in tesseract_examples --- .../src/basic_cartesian_example.cpp | 3 +++ tesseract_examples/src/car_seat_example.cpp | 23 ++++++++++++------- .../src/freespace_hybrid_example.cpp | 9 +++++--- .../src/freespace_ompl_example.cpp | 8 ++++--- .../src/glass_upright_example.cpp | 18 ++++++++++----- .../src/pick_and_place_example.cpp | 6 ++++- .../puzzle_piece_auxillary_axes_example.cpp | 6 ++++- .../src/puzzle_piece_example.cpp | 6 ++++- 8 files changed, 56 insertions(+), 23 deletions(-) diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 9900785aba8..ef508bedc74 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -201,6 +201,9 @@ bool BasicCartesianExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "RASTER", "freespace_profile" }); + addDefaultTaskComposerProfiles(*profiles, { "RASTER", "freespace_profile" }); + if (ifopt_) { auto composite_profile = std::make_shared(); diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index a00539a4d62..281a6d2edef 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -262,15 +262,21 @@ bool CarSeatExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_composite_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, trajopt_solver_profile); // Solve Trajectory CONSOLE_BRIDGE_logInform("Car Seat Demo Started"); { // Create Program to pick up first seat - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "end_effector")); + CompositeInstruction program(DEFAULT_PROFILE_KEY, + CompositeInstructionOrder::ORDERED, + ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Pick up the first seat!"); // Start and End Joint Position for the program @@ -284,7 +290,7 @@ bool CarSeatExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program @@ -358,8 +364,9 @@ bool CarSeatExample::run() return false; { // Create Program to place first seat - CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "world", "end_effector")); + CompositeInstruction program(DEFAULT_PROFILE_KEY, + CompositeInstructionOrder::ORDERED, + ManipulatorInfo("manipulator", "world", "end_effector")); program.setDescription("Place the first seat!"); // Start and End Joint Position for the program @@ -373,7 +380,7 @@ bool CarSeatExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("Freespace pick seat 1"); // Add Instructions to program diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index b341c6e4433..3712e08c744 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -138,7 +138,7 @@ bool FreespaceHybridExample::run() // Create Program CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -148,7 +148,7 @@ bool FreespaceHybridExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -172,7 +172,10 @@ bool FreespaceHybridExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, ompl_profile); // Create task TaskComposerNode::UPtr task = factory.createTaskComposerNode("FreespacePipeline"); diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 5f58f191b06..ee7e4803e86 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -138,7 +138,7 @@ bool FreespaceOMPLExample::run() // Create Program CompositeInstruction program( - "FREESPACE", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -148,7 +148,7 @@ bool FreespaceOMPLExample::run() start_instruction.setDescription("Start Instruction"); // Plan freespace from start - MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE, "FREESPACE"); + MoveInstruction plan_f0(wp1, MoveInstructionType::FREESPACE); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -172,7 +172,9 @@ bool FreespaceOMPLExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, ompl_profile); // Create task TaskComposerNode::UPtr task = factory.createTaskComposerNode("OMPLPipeline"); diff --git a/tesseract_examples/src/glass_upright_example.cpp b/tesseract_examples/src/glass_upright_example.cpp index de738379e29..4a4b63362fc 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -151,7 +151,7 @@ bool GlassUprightExample::run() // Create Program CompositeInstruction program( - "UPRIGHT", CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); + DEFAULT_PROFILE_KEY, CompositeInstructionOrder::ORDERED, ManipulatorInfo("manipulator", "base_link", "tool0")); // Start and End Joint Position for the program StateWaypointPoly wp0{ StateWaypoint(joint_names, joint_start_pos) }; @@ -162,7 +162,7 @@ bool GlassUprightExample::run() // Plan freespace from start // Assign a linear motion so cartesian is defined as the target - MoveInstruction plan_f0(wp1, MoveInstructionType::LINEAR, "UPRIGHT"); + MoveInstruction plan_f0(wp1, MoveInstructionType::LINEAR); plan_f0.setDescription("freespace_plan"); // Add Instructions to program @@ -177,6 +177,9 @@ bool GlassUprightExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + addDefaultTaskComposerProfiles(*profiles, { DEFAULT_PROFILE_KEY }); + if (ifopt_) { auto composite_profile = std::make_shared(); @@ -191,7 +194,8 @@ bool GlassUprightExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, composite_profile); auto plan_profile = std::make_shared(); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); @@ -199,7 +203,8 @@ bool GlassUprightExample::run() plan_profile->cartesian_coeff(0) = 0; plan_profile->cartesian_coeff(1) = 0; plan_profile->cartesian_coeff(2) = 0; - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile( + TRAJOPT_IFOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, plan_profile); } else { @@ -218,7 +223,8 @@ bool GlassUprightExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_coeff = Eigen::VectorXd::Constant(6, 1, 5); @@ -227,7 +233,7 @@ bool GlassUprightExample::run() plan_profile->cartesian_coeff(2) = 0; // Add profile to Dictionary - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, DEFAULT_PROFILE_KEY, plan_profile); } // Create task diff --git a/tesseract_examples/src/pick_and_place_example.cpp b/tesseract_examples/src/pick_and_place_example.cpp index 4e79dff657f..7c30b1b8ac8 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -212,8 +212,12 @@ bool PickAndPlaceExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "DEFAULT", "CARTESIAN", "FREESPACE" }); + addDefaultTaskComposerProfiles(*profiles, { "DEFAULT", "CARTESIAN", "FREESPACE" }); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index ae9a854e10c..948dfc016d8 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -220,8 +220,12 @@ bool PuzzlePieceAuxillaryAxesExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "CARTESIAN", "DEFAULT" }); + addDefaultTaskComposerProfiles(*profiles, { "CARTESIAN", "DEFAULT" }); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create task diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index f8f06f1d612..20b9745f06c 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -212,8 +212,12 @@ bool PuzzlePieceExample::run() // Create profile dictionary auto profiles = std::make_shared(); + addDefaultPlannerProfiles(*profiles, { "DEFAULT", "CARTESIAN" }); + addDefaultTaskComposerProfiles(*profiles, { "DEFAULT", "CARTESIAN" }); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile( + TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); // Create task From e424d03f22456a1f9322474adbaecb4e48eec822 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 12 Oct 2022 15:42:12 -0500 Subject: [PATCH 15/17] Clang-tidy fixes --- .../src/composite_instruction.cpp | 4 ++-- .../src/move_instruction.cpp | 8 ++++---- .../test/command_language_unit.cpp | 2 +- .../test/profile_dictionary_tests.cpp | 14 +++++++------- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index 2f44e0b36ac..6d31fc73ce2 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -83,8 +83,8 @@ const std::string& CompositeInstruction::getProfile(const std::string& ns) const { if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) return profile_; - else - return profile_overrides_.at(ns); + + return profile_overrides_.at(ns); } void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides) diff --git a/tesseract_command_language/src/move_instruction.cpp b/tesseract_command_language/src/move_instruction.cpp index 6a3a9d33b69..c622958bf2e 100644 --- a/tesseract_command_language/src/move_instruction.cpp +++ b/tesseract_command_language/src/move_instruction.cpp @@ -194,8 +194,8 @@ const std::string& MoveInstruction::getProfile(const std::string& ns) const { if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) return profile_; - else - return profile_overrides_.at(ns); + + return profile_overrides_.at(ns); } void MoveInstruction::setPathProfile(const std::string& profile) { path_profile_ = profile; } @@ -203,8 +203,8 @@ const std::string& MoveInstruction::getPathProfile(const std::string& ns) const { if (ns.empty() || (path_profile_overrides_.find(ns) == path_profile_overrides_.end())) return path_profile_; - else - return path_profile_overrides_.at(ns); + + return path_profile_overrides_.at(ns); } void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides) diff --git a/tesseract_command_language/test/command_language_unit.cpp b/tesseract_command_language/test/command_language_unit.cpp index 2727340e04c..8c8a818ca53 100644 --- a/tesseract_command_language/test/command_language_unit.cpp +++ b/tesseract_command_language/test/command_language_unit.cpp @@ -673,7 +673,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT } } -TEST(TesseractCommandLanguageMoveInstructionPolyUnit, ProfileOverrides) +TEST(TesseractCommandLanguageMoveInstructionPolyUnit, ProfileOverrides) // NOLINT { Eigen::VectorXd jv = Eigen::VectorXd::Ones(6); std::vector jn = { "j1", "j2", "j3", "j4", "j5", "j6" }; diff --git a/tesseract_motion_planners/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/test/profile_dictionary_tests.cpp index 093ee2ae24e..172f1f0f1f8 100644 --- a/tesseract_motion_planners/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/test/profile_dictionary_tests.cpp @@ -60,12 +60,12 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_THROW(profiles.getProfile("ns", "key"), std::runtime_error); + EXPECT_THROW(profiles.getProfile("ns", "key"), std::runtime_error); // NOLINT profiles.addProfile("ns", "key", std::make_shared()); ProfileBase::ConstPtr profile; - ASSERT_NO_THROW(profile = profiles.getProfile("ns", "key")); + ASSERT_NO_THROW(profile = profiles.getProfile("ns", "key")); // NOLINT ASSERT_TRUE(profile != nullptr); EXPECT_EQ(profile->a, 0); @@ -73,14 +73,14 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT // Check add same profile with different key profiles.addProfile("ns", "key2", profile); ProfileBase::ConstPtr profile2; - ASSERT_NO_THROW(profile2 = profiles.getProfile("ns", "key2")); + ASSERT_NO_THROW(profile2 = profiles.getProfile("ns", "key2")); // NOLINT ASSERT_TRUE(profile2 != nullptr); EXPECT_EQ(profile2->a, 0); // Check replacing a profile profiles.addProfile("ns", "key", std::make_shared(10)); ProfileBase::ConstPtr profile_check; - ASSERT_NO_THROW(profile_check = profiles.getProfile("ns", "key")); + ASSERT_NO_THROW(profile_check = profiles.getProfile("ns", "key")); // NOLINT ASSERT_TRUE(profile_check != nullptr); EXPECT_EQ(profile_check->a, 10); @@ -91,7 +91,7 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT profiles.addProfile("ns", "key", std::make_shared(20)); ProfileBase::ConstPtr profile_check2; - ASSERT_NO_THROW(profile_check2 = profiles.getProfile("ns", "key")); + ASSERT_NO_THROW(profile_check2 = profiles.getProfile("ns", "key")); // NOLINT ASSERT_TRUE(profile_check2 != nullptr); EXPECT_EQ(profile_check2->a, 20); @@ -117,12 +117,12 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT profiles.addProfile("ns", "key", std::make_shared(5)); EXPECT_TRUE(profiles.hasProfileEntry("ns")); ProfileBase2::ConstPtr profile_check3; - ASSERT_NO_THROW(profile_check3 = profiles.getProfile("ns", "key")); + ASSERT_NO_THROW(profile_check3 = profiles.getProfile("ns", "key")); // NOLINT ASSERT_TRUE(profile_check3 != nullptr); EXPECT_EQ(profile_check3->b, 5); // Check that other profile entry with same key is not affected ProfileBase::ConstPtr profile_check4; - ASSERT_NO_THROW(profile_check4 = profiles.getProfile("ns", "key")); + ASSERT_NO_THROW(profile_check4 = profiles.getProfile("ns", "key")); // NOLINT ASSERT_TRUE(profile_check4 != nullptr); EXPECT_EQ(profile_check4->a, 20); } From 1250fe928d6af63a2f9b9da9bfe3619ae4840237 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 23 Oct 2022 00:21:13 -0500 Subject: [PATCH 16/17] Add missing profiles fix basic cartesian example --- tesseract_examples/src/basic_cartesian_example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index ef508bedc74..7f5195f4679 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -201,8 +201,8 @@ bool BasicCartesianExample::run() // Create profile dictionary auto profiles = std::make_shared(); - addDefaultPlannerProfiles(*profiles, { "RASTER", "freespace_profile" }); - addDefaultTaskComposerProfiles(*profiles, { "RASTER", "freespace_profile" }); + addDefaultPlannerProfiles(*profiles, { "RASTER", "freespace_profile", "cartesian_program" }); + addDefaultTaskComposerProfiles(*profiles, { "RASTER", "freespace_profile", "cartesian_program" }); if (ifopt_) { From 8ad98e7385a2c0e6d7561e6a81ae7a6f3efef401 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 25 Oct 2022 09:22:22 -0500 Subject: [PATCH 17/17] Update trajopt_ifopt_motion_planner.cpp --- .../trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index 9d2e3b6537f..23aafe69592 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp @@ -301,10 +301,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co fixed_steps.push_back(i); } } - else if (move_instruction.getWaypoint().isStateWaypoint()) // solver_profile = applyProfileOverrides(name, - // solver_profile, profile_overrides); - // if (!solver_profile) - // throw std::runtime_error("TrajOptSolverConfig: Invalid profile"); + else if (move_instruction.getWaypoint().isStateWaypoint()) { assert(checkJointPositionFormat(joint_names, move_instruction.getWaypoint())); const auto& swp = move_instruction.getWaypoint().as();