diff --git a/.github/ISSUE_TEMPLATE/bug-report.yml b/.github/ISSUE_TEMPLATE/bug-report.yml new file mode 100644 index 00000000000..092f487cb78 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug-report.yml @@ -0,0 +1,57 @@ +name: Bug Report +description: Create a report to help us improve +body: + - type: input + attributes: + label: Version + description: What version of `tesseract_planning` are you using? + placeholder: Post a tag (e.g., `0.25.0`) or commit hash (e.g., `12faec5`) + validations: + required: True + + - type: dropdown + attributes: + label: OS Version + description: What OS version are you running? + options: + - Ubuntu 20.04 + - Ubuntu 22.04 + - Ubuntu 24.04 + - MacOS 12 + - MacOS 14 + - Windows (MSVC 2019) + - Windows (MSVC 2022) + - Other (please specify in the bug description) + validations: + required: True + + - type: textarea + attributes: + label: Describe the bug + placeholder: | + A clear and concise description of the bug + validations: + required: True + + - type: textarea + attributes: + label: To Reproduce + placeholder: | + Describe the steps to reproduce the behavior + validations: + required: True + + - type: textarea + attributes: + label: Expected behavior + placeholder: | + A clear and concise description of what you expected to happen. + validations: + required: True + + - type: textarea + attributes: + label: Relevant log output + placeholder: | + Paste any relevant log output here (will be rendered as shell text) + render: shell diff --git a/.github/ISSUE_TEMPLATE/feature-request.yml b/.github/ISSUE_TEMPLATE/feature-request.yml new file mode 100644 index 00000000000..3c90dccf65e --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature-request.yml @@ -0,0 +1,32 @@ +name: Feature Request +description: Suggest an idea for this project +body: + - type: textarea + attributes: + label: Is your feature request related to a problem? Please describe. + placeholder: | + A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] + validations: + required: True + + - type: textarea + attributes: + label: Describe the solution you'd like + placeholder: | + A clear and concise description of what you want to happen. + validations: + required: True + + - type: textarea + attributes: + label: Describe alternatives you've considered + placeholder: | + A clear and concise description of any alternative solutions or features you've considered. + validations: + required: True + + - type: textarea + attributes: + label: Additional context + placeholder: | + Add any other context or screenshots about the feature request here. diff --git a/.github/workflows/mac.yml b/.github/workflows/mac.yml index c37354106e1..5290260b641 100644 --- a/.github/workflows/mac.yml +++ b/.github/workflows/mac.yml @@ -35,7 +35,7 @@ jobs: fail-fast: false matrix: config: - - runner: macos-12 + - runner: macos-13 vcpkg_triplet: x64-osx-dynamic-release arch: x64 homebrew_root: /usr/local diff --git a/.github/workflows/windows.yml b/.github/workflows/windows.yml index 3afc257e02e..abcd268e231 100644 --- a/.github/workflows/windows.yml +++ b/.github/workflows/windows.yml @@ -59,5 +59,5 @@ jobs: vcs-file: .github/workflows/windows_dependencies.repos upstream-args: --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=x64-windows-release -DCMAKE_BUILD_TYPE=Release -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF -DTESSERACT_BUILD_TRAJOPT_IFOPT=OFF -DVCPKG_APPLOCAL_DEPS=OFF target-path: target_ws/src - target-args: --packages-ignore tesseract_examples --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=x64-windows-release -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_BUILD_TRAJOPT_IFOPT=OFF -DVCPKG_APPLOCAL_DEPS=OFF + target-args: --event-handlers console_cohesion+ --packages-ignore tesseract_examples --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=x64-windows-release -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DTESSERACT_BUILD_TRAJOPT_IFOPT=OFF -DVCPKG_APPLOCAL_DEPS=OFF run-tests-args: --packages-ignore tesseract_examples diff --git a/tesseract_command_language/CMakeLists.txt b/tesseract_command_language/CMakeLists.txt index b2db2633c0e..80d199d2f9f 100644 --- a/tesseract_command_language/CMakeLists.txt +++ b/tesseract_command_language/CMakeLists.txt @@ -49,6 +49,7 @@ add_library( src/poly/waypoint_poly.cpp src/move_instruction.cpp src/profile_dictionary.cpp + src/profile.cpp src/set_analog_instruction.cpp src/set_tool_instruction.cpp src/timer_instruction.cpp 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 475d42b69d5..a76c0cc8da8 100644 --- a/tesseract_command_language/include/tesseract_command_language/composite_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/composite_instruction.h @@ -37,13 +37,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include namespace tesseract_planning { class CompositeInstruction; -class ProfileDictionary; struct MoveInstructionPoly; /** @@ -134,10 +134,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(std::shared_ptr profile_overrides); - std::shared_ptr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides& getProfileOverrides() const; void setManipulatorInfo(tesseract_common::ManipulatorInfo info); const tesseract_common::ManipulatorInfo& getManipulatorInfo() const; @@ -409,7 +409,7 @@ class CompositeInstruction std::string profile_{ DEFAULT_PROFILE_KEY }; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - std::shared_ptr 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/fwd.h b/tesseract_command_language/include/tesseract_command_language/fwd.h index 16fdaad265b..c800a38f02b 100644 --- a/tesseract_command_language/include/tesseract_command_language/fwd.h +++ b/tesseract_command_language/include/tesseract_command_language/fwd.h @@ -31,6 +31,7 @@ enum class WaitInstructionType : int; class WaitInstruction; // Profile Dictionary +class Profile; class ProfileDictionary; } // namespace tesseract_planning #endif // TESSERACT_COMMAND_LANGUAGE_FWD_H 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 c3126ba9b69..50caae6f3c9 100644 --- a/tesseract_command_language/include/tesseract_command_language/move_instruction.h +++ b/tesseract_command_language/include/tesseract_command_language/move_instruction.h @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include namespace tesseract_planning @@ -139,16 +140,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(std::shared_ptr profile_overrides); - std::shared_ptr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides& getProfileOverrides() const; - void setPathProfileOverrides(std::shared_ptr profile_overrides); - std::shared_ptr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides& getPathProfileOverrides() const; const std::string& getDescription() const; @@ -183,10 +184,10 @@ class MoveInstruction std::string path_profile_; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - std::shared_ptr profile_overrides_; + ProfileOverrides profile_overrides_; /** @brief Dictionary of path profiles that will override named profiles for a specific task*/ - std::shared_ptr 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 0058c670db3..8db53b945ac 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,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -127,11 +128,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); @@ -180,16 +181,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(std::shared_ptr profile_overrides) = 0; - virtual std::shared_ptr getProfileOverrides() const = 0; + virtual void setProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual const ProfileOverrides& getProfileOverrides() const = 0; - virtual void setPathProfileOverrides(std::shared_ptr profile_overrides) = 0; - virtual std::shared_ptr getPathProfileOverrides() const = 0; + virtual void setPathProfileOverrides(ProfileOverrides profile_overrides) = 0; + virtual const ProfileOverrides& getPathProfileOverrides() const = 0; virtual void setMoveType(MoveInstructionType move_type) = 0; virtual MoveInstructionType getMoveType() const = 0; @@ -244,28 +245,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(std::shared_ptr profile_overrides) final + void setProfileOverrides(ProfileOverrides profile_overrides) final { - this->get().setProfileOverrides(profile_overrides); - } - std::shared_ptr getProfileOverrides() const final - { - return this->get().getProfileOverrides(); + this->get().setProfileOverrides(std::move(profile_overrides)); } + const ProfileOverrides& getProfileOverrides() const final { return this->get().getProfileOverrides(); } - void setPathProfileOverrides(std::shared_ptr profile_overrides) final - { - this->get().setPathProfileOverrides(profile_overrides); - } - std::shared_ptr getPathProfileOverrides() const final + void setPathProfileOverrides(ProfileOverrides profile_overrides) final { - return this->get().getPathProfileOverrides(); + this->get().setPathProfileOverrides(std::move(profile_overrides)); } + const 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(); } @@ -321,16 +316,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(std::shared_ptr profile_overrides); - std::shared_ptr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides& getProfileOverrides() const; - void setPathProfileOverrides(std::shared_ptr profile_overrides); - std::shared_ptr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides& getPathProfileOverrides() const; void setMoveType(MoveInstructionType move_type); MoveInstructionType getMoveType() const; diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h new file mode 100644 index 00000000000..2a97d726d2e --- /dev/null +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -0,0 +1,68 @@ +/** + * @file profile.h + * @brief This is a profile base class + * + * @author Levi Armstrong + * @date December 2, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_MOTION_PLANNERS_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_PROFILE_H + +#include +#include +#include + +namespace tesseract_planning +{ +/** + * @brief The Profile class + */ +class Profile +{ +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + Profile() = default; + Profile(std::size_t key); + Profile(const Profile&) = delete; + Profile& operator=(const Profile&) = delete; + Profile(Profile&&) = delete; + Profile&& operator=(Profile&&) = delete; + virtual ~Profile() = default; + + /** + * @brief Get the hash code associated with the profile + * @return The profile's hash code + */ + std::size_t getKey() const; + +protected: + std::size_t key_{ 0 }; + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::Profile) + +#endif // TESSERACT_MOTION_PLANNERS_PROFILE_H 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 0e3fb693b5a..6c7548fcb2f 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -28,35 +28,19 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include 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 + * @brief This class is used to store profiles used by various tasks * @details This is a thread safe class - * A ProfileEntry is a std::unordered_map> - * - The key is the profile name - * - Where std::shared_ptr is the profile - * The ProfleEntry is also stored in std::unordered_map where the key here is the std::type_index(typeid(T)) - * @note When adding a profile entry the T should be the base class type. */ class ProfileDictionary { @@ -64,55 +48,6 @@ class ProfileDictionary using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - /** - * @brief Check if a profile entry exists - * @param ns The namesspace to search under - * @return True if exists, otherwise false - */ - template - bool hasProfileEntry(const std::string& ns) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return false; - - return (it->second.find(std::type_index(typeid(ProfileType))) != it->second.end()); - } - - /** @brief Remove a profile entry */ - template - void removeProfileEntry(const std::string& ns) - { - std::unique_lock lock(mutex_); - - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return; - - it->second.erase(std::type_index(typeid(ProfileType))); - } - - /** - * @brief Get a profile entry - * @return The profile map associated with the profile entry - */ - template - std::unordered_map> getProfileEntry(const std::string& ns) const - { - std::shared_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - throw std::runtime_error("Profile namespace does not exist for '" + ns + "'!"); - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - return std::any_cast>&>(it2->second); - - throw std::runtime_error("Profile entry does not exist for type name '" + - std::string(std::type_index(typeid(ProfileType)).name()) + "' in namespace '" + ns + "'!"); - } - /** * @brief Add a profile * @details If the profile entry does not exist it will create one @@ -120,114 +55,75 @@ class ProfileDictionary * @param profile_name The profile name * @param profile The profile to add */ - template - void addProfile(const std::string& ns, const std::string& profile_name, std::shared_ptr profile) - { - if (ns.empty()) - throw std::runtime_error("Adding profile with an empty namespace!"); - - if (profile_name.empty()) - throw std::runtime_error("Adding profile with an empty string as the key!"); - - if (profile == nullptr) - throw std::runtime_error("Adding profile that is a nullptr"); - - std::unique_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - { - std::unordered_map> new_entry; - new_entry[profile_name] = profile; - profiles_[ns][std::type_index(typeid(ProfileType))] = new_entry; - } - else - { - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - { - std::any_cast>&>(it2->second)[profile_name] = - profile; - } - else - { - std::unordered_map> new_entry; - new_entry[profile_name] = profile; - it->second[std::type_index(typeid(ProfileType))] = new_entry; - } - } - } + void addProfile(const std::string& ns, const std::string& profile_name, const Profile::ConstPtr& profile); + void addProfile(const std::string& ns, + const std::vector& profile_names, + const Profile::ConstPtr& profile); /** * @brief Check if a profile exists * @details If profile entry does not exist it also returns false + * @param key The profile key + * @param ns The profile namespace + * @param profile_name The profile name * @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; - } + bool hasProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const; /** * @brief Get a profile by name * @details Check if the profile exist before calling this function, if missing an exception is thrown + * @param key The profile key + * @param ns The profile namespace * @param profile_name The profile name * @return The profile */ - template - 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 auto& profile_map = - std::any_cast>&>(it2); - return profile_map.at(profile_name); - } + Profile::ConstPtr getProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const; /** * @brief Remove a profile + * @param key The profile key + * @param ns The profile namespace * @param profile_name The profile to be removed */ - template - void removeProfile(const std::string& ns, const std::string& profile_name) - { - std::unique_lock lock(mutex_); - auto it = profiles_.find(ns); - if (it == profiles_.end()) - return; - - auto it2 = it->second.find(std::type_index(typeid(ProfileType))); - if (it2 != it->second.end()) - std::any_cast>&>(it2->second) - .erase(profile_name); - } + void removeProfile(std::size_t key, const std::string& ns, const std::string& profile_name); + + /** + * @brief Check if a profile entry exists + * @param key The profile key + * @param ns The profile namespace + * @return True if exists, otherwise false + */ + bool hasProfileEntry(std::size_t key, const std::string& ns) const; + + /** + * @brief Remove a profile entry + * @param key The profile key + * @param ns The profile namespace + */ + void removeProfileEntry(std::size_t key, const std::string& ns); + + /** + * @brief Get a profile entry + * @param key The profile key + * @param ns The profile namespace + * @return The profile map associated with the profile entry + */ + std::unordered_map getProfileEntry(std::size_t key, const std::string& ns) const; /** @brief Clear the dictionary */ void clear(); protected: - std::unordered_map> profiles_; mutable std::shared_mutex mutex_; + std::unordered_map>> + profiles_; friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT }; + } // namespace tesseract_planning BOOST_CLASS_EXPORT_KEY(tesseract_planning::ProfileDictionary) diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp index e599918da6c..73e0be72e7a 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/cartesian_waypoint_poly_unit.hpp @@ -63,169 +63,168 @@ void runCartesianWaypointTest() EXPECT_FALSE(wp.isToleranced()); } - { // Set/Get Transform - { // Test construction providing pose + { // Set/Get Transform + { // Test construction providing pose Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T(pose) }; - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); - } + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); +} - { // Test construction providing pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) }; - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5))); - EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5))); - EXPECT_TRUE(wp.isToleranced()); - } - - { // Test set pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T() }; - wp.setTransform(pose); - EXPECT_TRUE(wp.getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); - } - - { // Test assigning pose - Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; - pose.translation() = Eigen::Vector3d(1, 2, 3); - CartesianWaypointPoly wp{ T() }; - wp.getTransform() = pose; - EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose)); - EXPECT_FALSE(wp.isToleranced()); - } - } // namespace tesseract_planning::test_suite - - { // Test Set Tolerances - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isToleranced()); +{ // Test construction providing pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose, Eigen::VectorXd::Constant(3, -5), Eigen::VectorXd::Constant(3, 5)) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_TRUE(wp.getLowerTolerance().isApprox(Eigen::VectorXd::Constant(3, -5))); + EXPECT_TRUE(wp.getUpperTolerance().isApprox(Eigen::VectorXd::Constant(3, 5))); + EXPECT_TRUE(wp.isToleranced()); +} - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(wp.isToleranced()); +{ // Test set pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T() }; + wp.setTransform(pose); + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); +} - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT +{ // Test assigning pose + Eigen::Isometry3d pose{ Eigen::Isometry3d::Identity() }; + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T() }; + wp.getTransform() = pose; + EXPECT_TRUE(std::as_const(wp).getTransform().isApprox(pose)); + EXPECT_FALSE(wp.isToleranced()); +} +} // namespace tesseract_planning::test_suite - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT +{ // Test Set Tolerances + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); - EXPECT_FALSE(wp.isToleranced()); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_TRUE(wp.isToleranced()); + + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); +} + +{ // Test Equality and Serialization + { CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; +CartesianWaypointPoly wp2(wp1); // NOLINT +EXPECT_TRUE(wp1 == wp2); +EXPECT_TRUE(wp2 == wp1); +EXPECT_FALSE(wp2 != wp1); +runWaypointSerializationTest(wp1); +} +{ + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); + CartesianWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); +} +{ + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + wp1.getUpperTolerance().resize(3); + wp1.getUpperTolerance() << 1, 2, 3; + wp1.getLowerTolerance().resize(3); + wp1.getLowerTolerance() << -4, -5, -6; + CartesianWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); +} +// Not equal +{ + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; + wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1))); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); +} +{ + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; + wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); +} +{ + CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; + CartesianWaypointPoly wp2(wp1); + wp2.getUpperTolerance().resize(3); + wp2.getUpperTolerance() << 1, 2, 3; + wp2.getLowerTolerance().resize(3); + wp2.getLowerTolerance() << -4, -5, -6; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp2); +} +} + +{ // Set/Get Seed + tesseract_common::JointState seed_state; + seed_state.joint_names = { "joint_1", "joint_2", "joint_3" }; + seed_state.position.resize(3); + seed_state.position << .01, .02, .03; + seed_state.velocity.resize(3); + seed_state.velocity << .1, .2, .3; + seed_state.acceleration.resize(3); + seed_state.acceleration << 1, 2, 3; + + { // Test default construction pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state); } - { // Test Equality and Serialization - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - wp1.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); - CartesianWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - wp1.getUpperTolerance().resize(3); - wp1.getUpperTolerance() << 1, 2, 3; - wp1.getLowerTolerance().resize(3); - wp1.getLowerTolerance() << -4, -5, -6; - CartesianWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; - wp2.getTransform().rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0, 0, 1))); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); - } - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2{ T(Eigen::Isometry3d::Identity()) }; - wp2.getTransform().translate(Eigen::Vector3d(1e6, 0, 0)); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); - } - { - CartesianWaypointPoly wp1{ T(Eigen::Isometry3d::Identity()) }; - CartesianWaypointPoly wp2(wp1); - wp2.getUpperTolerance().resize(3); - wp2.getUpperTolerance() << 1, 2, 3; - wp2.getLowerTolerance().resize(3); - wp2.getLowerTolerance() << -4, -5, -6; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp2); - } + { // Test assigning pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.setSeed(seed_state); + EXPECT_TRUE(wp.hasSeed()); + EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); + } + + { // Test assigning pose + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.getSeed() = seed_state; + EXPECT_TRUE(wp.hasSeed()); + EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); } - { // Set/Get Seed - tesseract_common::JointState seed_state; - seed_state.joint_names = { "joint_1", "joint_2", "joint_3" }; - seed_state.position.resize(3); - seed_state.position << .01, .02, .03; - seed_state.velocity.resize(3); - seed_state.velocity << .1, .2, .3; - seed_state.acceleration.resize(3); - seed_state.acceleration << 1, 2, 3; - - { // Test default construction pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - EXPECT_FALSE(std::as_const(wp).getSeed() == seed_state); - } - - { // Test assigning pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.setSeed(seed_state); - EXPECT_TRUE(wp.hasSeed()); - EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); - } - - { // Test assigning pose - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - EXPECT_TRUE(std::as_const(wp).getSeed() == seed_state); - } - - { // Test clear seed - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - wp.clearSeed(); - EXPECT_FALSE(wp.hasSeed()); - } + { // Test clear seed + CartesianWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.hasSeed()); + wp.getSeed() = seed_state; + EXPECT_TRUE(wp.hasSeed()); + wp.clearSeed(); + EXPECT_FALSE(wp.hasSeed()); } } +} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_CARTESIAN_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp index ca119e4d089..5353ab1545b 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/joint_waypoint_poly_unit.hpp @@ -52,228 +52,227 @@ void runJointWaypointTest() EXPECT_FALSE(base.isStateWaypoint()); } - { // Test construction - { - JointWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_FALSE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - JointWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); - EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); - EXPECT_TRUE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); - EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); - EXPECT_TRUE(wp.isConstrained()); - } - { - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT - } - } // namespace tesseract_planning::test_suite + { // Test construction + { JointWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_FALSE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + JointWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(std::as_const(wp).getUpperTolerance().rows() == 0); + EXPECT_TRUE(std::as_const(wp).getLowerTolerance().rows() == 0); + EXPECT_TRUE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + JointWaypointPoly wp{ T(names, positions, lower_tol, uppert_tol) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getLowerTolerance().isApprox(lower_tol)); + EXPECT_TRUE(wp.getUpperTolerance().isApprox(uppert_tol)); + EXPECT_TRUE(wp.isConstrained()); +} +{ + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd lower_tol = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd uppert_tol = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(JointWaypointPoly{ T(names, positions, lower_tol, uppert_tol) }); // NOLINT +} +} // namespace tesseract_planning::test_suite - { // Test is constrained +{ // Test is constrained + JointWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isConstrained()); + wp.setIsConstrained(true); + EXPECT_TRUE(wp.isConstrained()); +} + +{ // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isConstrained()); - wp.setIsConstrained(true); - EXPECT_TRUE(wp.isConstrained()); + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); } - { // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set - JointWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); - } - - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); - } + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); } +} - { // Set/Get Positions - Eigen::VectorXd positions; - positions.resize(3); - positions << 1.0, 2.0, 3.0; - - { // Test set - JointWaypointPoly wp{ T() }; - wp.setPosition(positions); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - } +{ // Set/Get Positions + Eigen::VectorXd positions; + positions.resize(3); + positions << 1.0, 2.0, 3.0; - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getPosition() = positions; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); - } + { // Test set + JointWaypointPoly wp{ T() }; + wp.setPosition(positions); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); } - { // Test Set Tolerances + { // Test assigning JointWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.isToleranced()); + wp.getPosition() = positions; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + } +} - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(wp.isToleranced()); +{ // Test Set Tolerances + JointWaypointPoly wp{ T() }; + EXPECT_FALSE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_TRUE(wp.isToleranced()); - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); - EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, -5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); - EXPECT_FALSE(wp.isToleranced()); - } + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 5)); + EXPECT_ANY_THROW(wp.isToleranced()); // NOLINT + + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); +} - { // Test Equality and Serialization - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - JointWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, upper_tol, lower_tol; - positions.resize(3); - upper_tol.resize(3); - lower_tol.resize(3); - positions << 0, -1e6, 1e6; - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; +{ // Test Equality and Serialization + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + JointWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, upper_tol, lower_tol; + positions.resize(3); + upper_tol.resize(3); + lower_tol.resize(3); + positions << 0, -1e6, 1e6; + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; - JointWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - Eigen::VectorXd upper_tol, lower_tol; - upper_tol.resize(3); - lower_tol.resize(3); - upper_tol << 1, 2, 3; - lower_tol << -4, -5, -6; - JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - JointWaypointPoly wp2(wp1); - wp1.getUpperTolerance() = upper_tol; - wp1.getLowerTolerance() = lower_tol; - EXPECT_TRUE(wp1.isConstrained()); - EXPECT_TRUE(wp2.isConstrained()); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } + JointWaypointPoly wp1{ T(names, positions, lower_tol, upper_tol) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2{ T({ "j1", "j2", "j3" }, { 0.001, 0, 0 }) }; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }, { 1, 2, 3 }, { -4, -5, -6 }) }; + JointWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + Eigen::VectorXd upper_tol, lower_tol; + upper_tol.resize(3); + lower_tol.resize(3); + upper_tol << 1, 2, 3; + lower_tol << -4, -5, -6; + JointWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + JointWaypointPoly wp2(wp1); + wp1.getUpperTolerance() = upper_tol; + wp1.getLowerTolerance() = lower_tol; + EXPECT_TRUE(wp1.isConstrained()); + EXPECT_TRUE(wp2.isConstrained()); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } +} } } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_JOINT_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/move_instruction_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/move_instruction_poly_unit.hpp index 025d64307b1..56361187b3b 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/move_instruction_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/move_instruction_poly_unit.hpp @@ -103,8 +103,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -133,8 +133,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -163,8 +163,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_TRUE(instr.getPathProfile().empty()); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -193,8 +193,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR); EXPECT_EQ(instr.getProfile(), DEFAULT_PROFILE_KEY); EXPECT_EQ(instr.getPathProfile(), DEFAULT_PROFILE_KEY); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -224,8 +224,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -254,8 +254,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_TRUE(instr.getPathProfile().empty()); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -284,8 +284,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_TRUE(instr.getPathProfile().empty()); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -314,8 +314,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -345,8 +345,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::CIRCULAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -375,8 +375,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::FREESPACE); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -406,8 +406,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -436,8 +436,8 @@ void runMoveInstructionConstructorTest() EXPECT_EQ(instr.getMoveType(), MoveInstructionType::LINEAR); EXPECT_EQ(instr.getProfile(), "TEST_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); - EXPECT_TRUE(instr.getProfileOverrides() == nullptr); - EXPECT_TRUE(instr.getPathProfileOverrides() == nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); + EXPECT_TRUE(instr.getPathProfileOverrides().empty()); EXPECT_FALSE(instr.getDescription().empty()); EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); @@ -496,13 +496,28 @@ void runMoveInstructionSettersTest() instr.setPathProfile("TEST_PATH_PROFILE"); EXPECT_EQ(instr.getPathProfile(), "TEST_PATH_PROFILE"); - auto profiles = std::make_shared(); - instr.setProfileOverrides(profiles); - EXPECT_TRUE(instr.getProfileOverrides() == profiles); + // 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(ns1), ns1_profile); + EXPECT_EQ(instr.getProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getProfile("nonexistent_ns"), "TEST_PROFILE"); - auto path_profiles = std::make_shared(); - instr.setPathProfileOverrides(path_profiles); - EXPECT_TRUE(instr.getPathProfileOverrides() == path_profiles); + // Path Profile Overrides + EXPECT_EQ(instr.getPathProfile(ns1), ns1_profile); + EXPECT_EQ(instr.getPathProfile(ns2), ns2_profile); + EXPECT_EQ(instr.getPathProfile("nonexistent_ns"), "TEST_PATH_PROFILE"); EXPECT_TRUE(instr.getManipulatorInfo().empty()); tesseract_common::ManipulatorInfo manip_info("manip", "base_link", "tool0"); diff --git a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp index 991a77d5c57..08a14b851a8 100644 --- a/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp +++ b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp @@ -52,257 +52,256 @@ void runStateWaypointTest() EXPECT_TRUE(base.isStateWaypoint()); } - { // Test construction - { - StateWaypointPoly wp{ T() }; - EXPECT_TRUE(wp.getNames().empty()); - EXPECT_TRUE(wp.getPosition().rows() == 0); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - StateWaypointPoly wp{ T(names, positions) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().rows() == 0); - EXPECT_TRUE(wp.getAcceleration().rows() == 0); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; - EXPECT_EQ(wp.getNames(), names); - EXPECT_TRUE(wp.getPosition().isApprox(positions)); - EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); - EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); - EXPECT_TRUE(wp.getEffort().rows() == 0); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); - } - { - std::vector names{ "j1", "j2" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); - Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); - Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); - EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT - } - } // namespace tesseract_planning::test_suite - - { // Set/Get Names - const std::vector names{ "j1", "j2", "j3" }; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); - } + { // Test construction + { StateWaypointPoly wp{ T() }; + EXPECT_TRUE(wp.getNames().empty()); + EXPECT_TRUE(wp.getPosition().rows() == 0); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + StateWaypointPoly wp{ T(names, positions) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().rows() == 0); + EXPECT_TRUE(wp.getAcceleration().rows() == 0); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 0.0)); +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + StateWaypointPoly wp{ T(names, positions, velocities, accelerations, 5) }; + EXPECT_EQ(wp.getNames(), names); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + EXPECT_TRUE(wp.getVelocity().isApprox(velocities)); + EXPECT_TRUE(wp.getAcceleration().isApprox(accelerations)); + EXPECT_TRUE(wp.getEffort().rows() == 0); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 5)); +} +{ + std::vector names{ "j1", "j2" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(2, 0.0); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(2, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(3, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT +} +{ + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions = Eigen::VectorXd::Constant(3, 0.0); + Eigen::VectorXd velocities = Eigen::VectorXd::Constant(3, -5); + Eigen::VectorXd accelerations = Eigen::VectorXd::Constant(2, 5); + EXPECT_ANY_THROW(StateWaypointPoly{ T(names, positions, velocities, accelerations, 5) }); // NOLINT +} +} // namespace tesseract_planning::test_suite - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); - } +{ // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + StateWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); } - { // Set/Get Positions - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setPosition(data); - EXPECT_TRUE(wp.getPosition().isApprox(data)); - } +{ // Set/Get Positions + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getPosition() = data; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setPosition(data); + EXPECT_TRUE(wp.getPosition().isApprox(data)); } - { // Set/Get Velocity - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getPosition() = data; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setVelocity(data); - EXPECT_TRUE(wp.getVelocity().isApprox(data)); - } +{ // Set/Get Velocity + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getVelocity() = data; - EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setVelocity(data); + EXPECT_TRUE(wp.getVelocity().isApprox(data)); } - { // Set/Get Acceleration - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getVelocity() = data; + EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setAcceleration(data); - EXPECT_TRUE(wp.getAcceleration().isApprox(data)); - } +{ // Set/Get Acceleration + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getAcceleration() = data; - EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setAcceleration(data); + EXPECT_TRUE(wp.getAcceleration().isApprox(data)); } - { // Set/Get Effort - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getAcceleration() = data; + EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + } +} - { // Test set - StateWaypointPoly wp{ T() }; - wp.setEffort(data); - EXPECT_TRUE(wp.getEffort().isApprox(data)); - } +{ // Set/Get Effort + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getEffort() = data; - EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setEffort(data); + EXPECT_TRUE(wp.getEffort().isApprox(data)); } - { // Set/Get Time - double data{ 3 }; + { // Test assigning StateWaypointPoly wp{ T() }; - wp.setTime(data); - EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); + wp.getEffort() = data; + EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); } +} + +{ // Set/Get Time + double data{ 3 }; + StateWaypointPoly wp{ T() }; + wp.setTime(data); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), data)); +} - { // Test Equality and Serialization - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; - const StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; - StateWaypointPoly wp2(wp1); // NOLINT - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; +{ // Test Equality and Serialization + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0.0, 0.0, 0.0 }) }; + const StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }) }; + StateWaypointPoly wp2(wp1); // NOLINT + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - { - Eigen::VectorXd efforts; - efforts.resize(3); - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T(names, positions, velocities, accelerations, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + { + Eigen::VectorXd efforts; + efforts.resize(3); + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; - wp1.getEffort() = efforts; - StateWaypointPoly wp2(wp1); - EXPECT_TRUE(wp1 == wp2); - EXPECT_TRUE(wp2 == wp1); - EXPECT_FALSE(wp2 != wp1); - runWaypointSerializationTest(wp1); - } - // Not equal - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } - { - std::vector names{ "j1", "j2", "j3" }; - Eigen::VectorXd positions, velocities, accelerations, efforts; - positions.resize(3); - velocities.resize(3); - accelerations.resize(3); - efforts.resize(3); - positions << 0, -1e6, 1e6; - velocities << 1, 2, 3; - accelerations << -4, -5, -6; - efforts << 4, 5, 6; + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, -1e6, 1e6 }, { 1, 2, 3 }, { -4, -5, -6 }, 5) }; + wp1.getEffort() = efforts; + StateWaypointPoly wp2(wp1); + EXPECT_TRUE(wp1 == wp2); + EXPECT_TRUE(wp2 == wp1); + EXPECT_FALSE(wp2 != wp1); + runWaypointSerializationTest(wp1); + } + // Not equal + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T(std::vector({ "j1" }), Eigen::VectorXd::Zero(1)) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ T({ "j1", "j2", "j4" }, { 0, 0, 0 }) }; + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); + } + { + std::vector names{ "j1", "j2", "j3" }; + Eigen::VectorXd positions, velocities, accelerations, efforts; + positions.resize(3); + velocities.resize(3); + accelerations.resize(3); + efforts.resize(3); + positions << 0, -1e6, 1e6; + velocities << 1, 2, 3; + accelerations << -4, -5, -6; + efforts << 4, 5, 6; - StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; - StateWaypointPoly wp2{ wp1 }; - wp1.getNames() = names; - wp1.getPosition() = positions; - wp1.getVelocity() = velocities; - wp1.getAcceleration() = accelerations; - wp1.getEffort() = efforts; - wp1.setTime(5); - EXPECT_FALSE(wp1 == wp2); - EXPECT_FALSE(wp2 == wp1); - EXPECT_TRUE(wp2 != wp1); - runWaypointSerializationTest(wp1); - runWaypointSerializationTest(wp2); - } + StateWaypointPoly wp1{ T({ "j1", "j2", "j3" }, { 0, 0, 0 }) }; + StateWaypointPoly wp2{ wp1 }; + wp1.getNames() = names; + wp1.getPosition() = positions; + wp1.getVelocity() = velocities; + wp1.getAcceleration() = accelerations; + wp1.getEffort() = efforts; + wp1.setTime(5); + EXPECT_FALSE(wp1 == wp2); + EXPECT_FALSE(wp2 == wp1); + EXPECT_TRUE(wp2 != wp1); + runWaypointSerializationTest(wp1); + runWaypointSerializationTest(wp2); } } +} } // namespace tesseract_planning::test_suite #endif // TESSERACT_COMMAND_LANGUAGE_STATE_WAYPOINT_POLY_UNIT_HPP diff --git a/tesseract_command_language/include/tesseract_command_language/types.h b/tesseract_command_language/include/tesseract_command_language/types.h new file mode 100644 index 00000000000..10eceaa3d3d --- /dev/null +++ b/tesseract_command_language/include/tesseract_command_language/types.h @@ -0,0 +1,45 @@ +/** + * @file types.h + * @brief Contains common types used throughout command language + * + * @author Levi Armstrong + * @date July 22, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COMMAND_LANGUAGE_TYPES_H +#define TESSERACT_COMMAND_LANGUAGE_TYPES_H + +#include +#include + +namespace tesseract_planning +{ +/** + * @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 diff --git a/tesseract_command_language/src/composite_instruction.cpp b/tesseract_command_language/src/composite_instruction.cpp index c0b3e9dc08d..9d1c63ec3af 100644 --- a/tesseract_command_language/src/composite_instruction.cpp +++ b/tesseract_command_language/src/composite_instruction.cpp @@ -82,16 +82,19 @@ void CompositeInstruction::setProfile(const std::string& profile) { profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile; } -const std::string& CompositeInstruction::getProfile() const { return profile_; } - -void CompositeInstruction::setProfileOverrides(std::shared_ptr profile_overrides) +const std::string& CompositeInstruction::getProfile(const std::string& ns) const { - profile_overrides_ = std::move(profile_overrides); + if (ns.empty() || (profile_overrides_.find(ns) == profile_overrides_.end())) + return profile_; + + return profile_overrides_.at(ns); } -std::shared_ptr CompositeInstruction::getProfileOverrides() const + +void CompositeInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { - return profile_overrides_; + profile_overrides_ = std::move(profile_overrides); } +const 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 101297b7368..e68a6a8f97a 100644 --- a/tesseract_command_language/src/move_instruction.cpp +++ b/tesseract_command_language/src/move_instruction.cpp @@ -37,7 +37,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include namespace tesseract_planning { @@ -191,25 +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_; + + 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_; -void MoveInstruction::setProfileOverrides(std::shared_ptr profile_overrides) + return path_profile_overrides_.at(ns); +} + +void MoveInstruction::setProfileOverrides(ProfileOverrides profile_overrides) { profile_overrides_ = std::move(profile_overrides); } -std::shared_ptr MoveInstruction::getProfileOverrides() const { return profile_overrides_; } +const ProfileOverrides& MoveInstruction::getProfileOverrides() const { return profile_overrides_; } -void MoveInstruction::setPathProfileOverrides(std::shared_ptr profile_overrides) +void MoveInstruction::setPathProfileOverrides(ProfileOverrides profile_overrides) { path_profile_overrides_ = std::move(profile_overrides); } -std::shared_ptr MoveInstruction::getPathProfileOverrides() const -{ - return path_profile_overrides_; -} +const 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 f1918ce42a2..b44533789c4 100644 --- a/tesseract_command_language/src/poly/move_instruction_poly.cpp +++ b/tesseract_command_language/src/poly/move_instruction_poly.cpp @@ -71,35 +71,34 @@ 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( - std::shared_ptr profile_overrides) +void tesseract_planning::MoveInstructionPoly::setProfileOverrides(ProfileOverrides profile_overrides) { getInterface().setProfileOverrides(std::move(profile_overrides)); } -std::shared_ptr -tesseract_planning::MoveInstructionPoly::getProfileOverrides() const +const tesseract_planning::ProfileOverrides& tesseract_planning::MoveInstructionPoly::getProfileOverrides() const { return getInterface().getProfileOverrides(); } -void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides( - std::shared_ptr profile_overrides) +void tesseract_planning::MoveInstructionPoly::setPathProfileOverrides(ProfileOverrides profile_overrides) { getInterface().setPathProfileOverrides(std::move(profile_overrides)); } -std::shared_ptr -tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const +const tesseract_planning::ProfileOverrides& tesseract_planning::MoveInstructionPoly::getPathProfileOverrides() const { return getInterface().getPathProfileOverrides(); } diff --git a/tesseract_command_language/src/profile.cpp b/tesseract_command_language/src/profile.cpp new file mode 100644 index 00000000000..52f1eee0b6e --- /dev/null +++ b/tesseract_command_language/src/profile.cpp @@ -0,0 +1,46 @@ +/** + * @file profile.cpp + * @brief This is a profile base class + * + * @author Levi Armstrong + * @date December 2, 2024 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2024, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +Profile::Profile(std::size_t key) : key_(key) {} + +std::size_t Profile::getKey() const { return key_; } + +template +void Profile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("key", key_); +} +} // namespace tesseract_planning + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::Profile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::Profile) diff --git a/tesseract_command_language/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index 50657397a93..c27f9660acb 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -25,19 +25,189 @@ */ #include +#include #include +#include namespace tesseract_planning { +bool ProfileDictionary::hasProfileEntry(std::size_t key, const std::string& ns) const +{ + const std::shared_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + return false; + + return (it->second.find(key) != it->second.end()); +} + +void ProfileDictionary::removeProfileEntry(std::size_t key, const std::string& ns) +{ + const std::unique_lock lock(mutex_); + + auto it = profiles_.find(ns); + if (it == profiles_.end()) + return; + + it->second.erase(key); +} + +std::unordered_map ProfileDictionary::getProfileEntry(std::size_t key, + const std::string& ns) const +{ + const std::shared_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + throw std::runtime_error("Profile namespace does not exist for '" + ns + "'!"); + + auto it2 = it->second.find(key); + if (it2 != it->second.end()) + return it2->second; + + throw std::runtime_error("Profile entry does not exist for type name '" + std::to_string(key) + "' in namespace '" + + ns + "'!"); +} + +void ProfileDictionary::addProfile(const std::string& ns, + const std::string& profile_name, + const Profile::ConstPtr& profile) +{ + if (ns.empty()) + throw std::runtime_error("Adding profile with an empty namespace!"); + + if (profile_name.empty()) + throw std::runtime_error("Adding profile with an empty string as the key!"); + + if (profile == nullptr) + throw std::runtime_error("Adding profile that is a nullptr"); + + const std::unique_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + { + std::unordered_map new_entry; + new_entry[profile_name] = profile; + profiles_[ns][profile->getKey()] = new_entry; + } + else + { + auto it2 = it->second.find(profile->getKey()); + if (it2 != it->second.end()) + { + it2->second[profile_name] = profile; + } + else + { + std::unordered_map new_entry; + new_entry[profile_name] = profile; + it->second[profile->getKey()] = new_entry; + } + } +} + +void ProfileDictionary::addProfile(const std::string& ns, + const std::vector& profile_names, + const Profile::ConstPtr& profile) +{ + if (ns.empty()) + throw std::runtime_error("Adding profile with an empty namespace!"); + + if (profile_names.empty()) + throw std::runtime_error("Adding profile with an empty vector of keys!"); + + if (profile == nullptr) + throw std::runtime_error("Adding profile that is a nullptr"); + + const std::unique_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + { + std::unordered_map new_entry; + for (const auto& profile_name : profile_names) + { + if (profile_name.empty()) + throw std::runtime_error("Adding profile with an empty string as the key!"); + + new_entry[profile_name] = profile; + } + profiles_[ns][profile->getKey()] = new_entry; + } + else + { + auto it2 = it->second.find(profile->getKey()); + if (it2 != it->second.end()) + { + for (const auto& profile_name : profile_names) + { + if (profile_name.empty()) + throw std::runtime_error("Adding profile with an empty string as the key!"); + + it2->second[profile_name] = profile; + } + } + else + { + std::unordered_map new_entry; + for (const auto& profile_name : profile_names) + { + if (profile_name.empty()) + throw std::runtime_error("Adding profile with an empty string as the key!"); + + new_entry[profile_name] = profile; + } + it->second[profile->getKey()] = new_entry; + } + } +} + +bool ProfileDictionary::hasProfile(std::size_t key, const std::string& ns, const std::string& profile_name) const +{ + const std::shared_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + return false; + + auto it2 = it->second.find(key); + if (it2 != it->second.end()) + { + auto it3 = it2->second.find(profile_name); + if (it3 != it2->second.end()) + return true; + } + return false; +} + +Profile::ConstPtr ProfileDictionary::getProfile(std::size_t key, + const std::string& ns, + const std::string& profile_name) const +{ + const std::shared_lock lock(mutex_); + return profiles_.at(ns).at(key).at(profile_name); +} + +void ProfileDictionary::removeProfile(std::size_t key, const std::string& ns, const std::string& profile_name) +{ + const std::unique_lock lock(mutex_); + auto it = profiles_.find(ns); + if (it == profiles_.end()) + return; + + auto it2 = it->second.find(key); + if (it2 != it->second.end()) + it2->second.erase(profile_name); +} + void ProfileDictionary::clear() { - std::unique_lock lock(mutex_); + const std::unique_lock lock(mutex_); profiles_.clear(); } template -void ProfileDictionary::serialize(Archive& /*ar*/, const unsigned int /*version*/) +void ProfileDictionary::serialize(Archive& ar, const unsigned int /*version*/) { + const std::shared_lock lock(mutex_); + ar& boost::serialization::make_nvp("profiles", profiles_); } } // namespace tesseract_planning diff --git a/tesseract_command_language/test/command_language_unit.cpp b/tesseract_command_language/test/command_language_unit.cpp index 910b688096d..557656d6f4b 100644 --- a/tesseract_command_language/test/command_language_unit.cpp +++ b/tesseract_command_language/test/command_language_unit.cpp @@ -405,7 +405,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(instr.getUUID().is_nil()); EXPECT_TRUE(instr.getParentUUID().is_nil()); EXPECT_EQ(instr.getProfile(), profile); - EXPECT_EQ(instr.getProfileOverrides(), nullptr); + EXPECT_TRUE(instr.getProfileOverrides().empty()); EXPECT_EQ(instr.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(instr).getManipulatorInfo(), manip_info); EXPECT_EQ(instr.getOrder(), order); @@ -435,7 +435,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(insert_program.getUUID().is_nil()); EXPECT_TRUE(insert_program.getParentUUID().is_nil()); EXPECT_EQ(insert_program.getProfile(), DEFAULT_PROFILE_KEY); - EXPECT_EQ(insert_program.getProfileOverrides(), nullptr); + EXPECT_TRUE(insert_program.getProfileOverrides().empty()); EXPECT_TRUE(insert_program.getManipulatorInfo().empty()); EXPECT_EQ(insert_program.getOrder(), order); EXPECT_NE(insert_program.size(), insert_program.getInstructionCount()); // Because of nested composites @@ -469,7 +469,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(assign_program.getUUID().is_nil()); EXPECT_TRUE(assign_program.getParentUUID().is_nil()); EXPECT_EQ(assign_program.getProfile(), profile); - EXPECT_EQ(assign_program.getProfileOverrides(), nullptr); + EXPECT_TRUE(assign_program.getProfileOverrides().empty()); EXPECT_EQ(assign_program.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(assign_program).getManipulatorInfo(), manip_info); EXPECT_EQ(assign_program.getOrder(), order); @@ -503,7 +503,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(copy_program.getUUID().is_nil()); EXPECT_TRUE(copy_program.getParentUUID().is_nil()); EXPECT_EQ(copy_program.getProfile(), profile); - EXPECT_EQ(copy_program.getProfileOverrides(), nullptr); + EXPECT_TRUE(copy_program.getProfileOverrides().empty()); EXPECT_EQ(copy_program.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(copy_program).getManipulatorInfo(), manip_info); EXPECT_EQ(copy_program.getOrder(), order); @@ -537,7 +537,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(empty_program.getUUID().is_nil()); EXPECT_TRUE(empty_program.getParentUUID().is_nil()); EXPECT_EQ(empty_program.getProfile(), profile); - EXPECT_EQ(empty_program.getProfileOverrides(), nullptr); + EXPECT_TRUE(empty_program.getProfileOverrides().empty()); EXPECT_EQ(empty_program.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(empty_program).getManipulatorInfo(), manip_info); EXPECT_EQ(empty_program.getOrder(), order); @@ -567,15 +567,13 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(empty_program == instr); EXPECT_TRUE(empty_program != instr); - auto profile_overrides = std::make_shared(); T set_program; set_program.setProfile(profile); set_program.setManipulatorInfo(manip_info); - set_program.setProfileOverrides(profile_overrides); EXPECT_FALSE(set_program.getUUID().is_nil()); EXPECT_TRUE(set_program.getParentUUID().is_nil()); EXPECT_EQ(set_program.getProfile(), profile); - EXPECT_EQ(set_program.getProfileOverrides(), profile_overrides); + EXPECT_TRUE(set_program.getProfileOverrides().empty()); EXPECT_EQ(set_program.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(set_program).getManipulatorInfo(), manip_info); EXPECT_EQ(set_program.getOrder(), order); @@ -611,7 +609,7 @@ TEST(TesseractCommandLanguageUnit, CompositeInstructionTests) // NOLINT EXPECT_FALSE(clear_program.getUUID().is_nil()); EXPECT_TRUE(clear_program.getParentUUID().is_nil()); EXPECT_EQ(clear_program.getProfile(), profile); - EXPECT_EQ(clear_program.getProfileOverrides(), nullptr); + EXPECT_TRUE(clear_program.getProfileOverrides().empty()); EXPECT_EQ(clear_program.getManipulatorInfo(), manip_info); EXPECT_EQ(std::as_const(clear_program).getManipulatorInfo(), manip_info); EXPECT_EQ(clear_program.getOrder(), order); diff --git a/tesseract_examples/src/basic_cartesian_example.cpp b/tesseract_examples/src/basic_cartesian_example.cpp index 2ff9517c4f0..d39d6f7df56 100644 --- a/tesseract_examples/src/basic_cartesian_example.cpp +++ b/tesseract_examples/src/basic_cartesian_example.cpp @@ -232,14 +232,13 @@ bool BasicCartesianExample::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, "cartesian_program", composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_coeff = Eigen::VectorXd::Ones(6); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } else { @@ -250,7 +249,7 @@ bool BasicCartesianExample::run() composite_profile->smooth_accelerations = false; composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "cartesian_program", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_cost_config.enabled = false; @@ -259,8 +258,8 @@ bool BasicCartesianExample::run() plan_profile->joint_cost_config.enabled = false; plan_profile->joint_constraint_config.enabled = true; plan_profile->joint_constraint_config.coeff = Eigen::VectorXd::Ones(7); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "RASTER", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "freespace_profile", plan_profile); } // Create task diff --git a/tesseract_examples/src/car_seat_example.cpp b/tesseract_examples/src/car_seat_example.cpp index d81eedcd115..ade2c1d4e03 100644 --- a/tesseract_examples/src/car_seat_example.cpp +++ b/tesseract_examples/src/car_seat_example.cpp @@ -311,12 +311,9 @@ bool CarSeatExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_solver_profile); } else { @@ -335,8 +332,8 @@ bool CarSeatExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_solver_profile); } // Solve Trajectory diff --git a/tesseract_examples/src/freespace_hybrid_example.cpp b/tesseract_examples/src/freespace_hybrid_example.cpp index caafb555133..c674c79d837 100644 --- a/tesseract_examples/src/freespace_hybrid_example.cpp +++ b/tesseract_examples/src/freespace_hybrid_example.cpp @@ -203,7 +203,7 @@ bool FreespaceHybridExample::run() ompl_profile->planning_time = planning_time_; ompl_profile->planners = { ompl_planner_config, ompl_planner_config }; - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); if (ifopt_) { @@ -217,8 +217,7 @@ bool FreespaceHybridExample::run() trajopt_ifopt_composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); trajopt_ifopt_composite_profile->jerk_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "FREESPACE", trajopt_ifopt_composite_profile); } // Create task diff --git a/tesseract_examples/src/freespace_ompl_example.cpp b/tesseract_examples/src/freespace_ompl_example.cpp index 6db760c4387..199407c2ab2 100644 --- a/tesseract_examples/src/freespace_ompl_example.cpp +++ b/tesseract_examples/src/freespace_ompl_example.cpp @@ -188,7 +188,7 @@ bool FreespaceOMPLExample::run() // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", ompl_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "FREESPACE", 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 fe1dbb2714f..5b0dfe88b17 100644 --- a/tesseract_examples/src/glass_upright_example.cpp +++ b/tesseract_examples/src/glass_upright_example.cpp @@ -211,7 +211,7 @@ bool GlassUprightExample::run() composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); auto plan_profile = std::make_shared(); plan_profile->joint_coeff = Eigen::VectorXd::Ones(7); @@ -219,7 +219,7 @@ 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, "UPRIGHT", plan_profile); } else { @@ -239,7 +239,7 @@ bool GlassUprightExample::run() composite_profile->smooth_jerks = false; composite_profile->velocity_coeff = Eigen::VectorXd::Ones(1); composite_profile->acceleration_coeff = Eigen::VectorXd::Ones(1); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", composite_profile); auto plan_profile = std::make_shared(); plan_profile->cartesian_cost_config.enabled = false; @@ -250,7 +250,7 @@ bool GlassUprightExample::run() plan_profile->cartesian_constraint_config.coeff(2) = 0; // Add profile to Dictionary - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "UPRIGHT", 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 ba05e644c91..1e013eafda2 100644 --- a/tesseract_examples/src/pick_and_place_example.cpp +++ b/tesseract_examples/src/pick_and_place_example.cpp @@ -258,12 +258,9 @@ bool PickAndPlaceExample::run() auto trajopt_ifopt_solver_profile = std::make_shared(); trajopt_ifopt_solver_profile->opt_info.max_iterations = 100; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -286,15 +283,15 @@ bool PickAndPlaceExample::run() auto trajopt_solver_profile = std::make_shared(); trajopt_solver_profile->opt_info.max_iter = 100; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } auto post_check_profile = std::make_shared(); - profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); - profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); + profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "CARTESIAN", post_check_profile); + profiles->addProfile(DISCRETE_CONTACT_CHECK_TASK_NAME, "DEFAULT", post_check_profile); CONSOLE_BRIDGE_logInform("Pick plan"); diff --git a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp index 7e71909b52b..3ab5620abd8 100644 --- a/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp +++ b/tesseract_examples/src/puzzle_piece_auxillary_axes_example.cpp @@ -257,12 +257,9 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -291,9 +288,9 @@ bool PuzzlePieceAuxillaryAxesExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } // Create task diff --git a/tesseract_examples/src/puzzle_piece_example.cpp b/tesseract_examples/src/puzzle_piece_example.cpp index 730e53ce3a7..94a1e1fea31 100644 --- a/tesseract_examples/src/puzzle_piece_example.cpp +++ b/tesseract_examples/src/puzzle_piece_example.cpp @@ -247,12 +247,9 @@ bool PuzzlePieceExample::run() trajopt_ifopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_ifopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); - profiles->addProfile( - TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_ifopt_plan_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_composite_profile); + profiles->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_ifopt_solver_profile); } else { @@ -277,9 +274,9 @@ bool PuzzlePieceExample::run() trajopt_solver_profile->opt_info.min_approx_improve = 1e-3; trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "CARTESIAN", trajopt_plan_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_solver_profile); } // Create task 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 e2e9f0910ca..696e76552c1 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 @@ -42,15 +42,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP 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 @@ -65,9 +56,6 @@ struct PlannerRequest /** @brief The environment */ std::shared_ptr env; - /** @brief The start state to use for planning */ - tesseract_scene_graph::SceneState env_state; - /** @brief The profile dictionary */ std::shared_ptr profiles; @@ -77,18 +65,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 }; 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 ac8fbe45785..a9ed459674e 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,38 +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 tesseract_common::AnyPoly& profile_remapping_poly, - std::string default_profile = DEFAULT_PROFILE_KEY) -{ - std::string results = profile; - if (profile.empty()) - results = std::move(default_profile); - - if (profile_remapping_poly.isNull()) - return results; - - // Check for remapping of profile - const auto& profile_remapping = profile_remapping_poly.as(); - 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 @@ -138,8 +106,9 @@ std::shared_ptr getProfile(const std::string& ns, const ProfileDictionary& profile_dictionary, std::shared_ptr default_profile = nullptr) { - if (profile_dictionary.hasProfile(ns, profile)) - return profile_dictionary.getProfile(ns, profile); + if (profile_dictionary.hasProfile(ProfileType::getStaticKey(), ns, profile)) + return std::static_pointer_cast( + profile_dictionary.getProfile(ProfileType::getStaticKey(), ns, profile)); CONSOLE_BRIDGE_logDebug("Profile '%s' was not found in namespace '%s' for type '%s'. Using default if available. " "Available " @@ -148,9 +117,9 @@ std::shared_ptr getProfile(const std::string& ns, ns.c_str(), typeid(ProfileType).name()); - if (profile_dictionary.hasProfileEntry(ns)) + if (profile_dictionary.hasProfileEntry(ProfileType::getStaticKey(), ns)) { - for (const auto& pair : profile_dictionary.getProfileEntry(ns)) + for (const auto& pair : profile_dictionary.getProfileEntry(ProfileType::getStaticKey(), ns)) { CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str()); } @@ -158,31 +127,6 @@ std::shared_ptr getProfile(const std::string& ns, 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 diff --git a/tesseract_motion_planners/core/test/CMakeLists.txt b/tesseract_motion_planners/core/test/CMakeLists.txt index 47432d49986..a96f5407743 100644 --- a/tesseract_motion_planners/core/test/CMakeLists.txt +++ b/tesseract_motion_planners/core/test/CMakeLists.txt @@ -17,27 +17,3 @@ target_code_coverage( add_gtest_discover_tests(${PROJECT_NAME}_profile_dictionary_unit) add_dependencies(${PROJECT_NAME}_profile_dictionary_unit ${PROJECT_NAME}_core) add_dependencies(run_tests ${PROJECT_NAME}_profile_dictionary_unit) - -# Utils Tests -add_executable(${PROJECT_NAME}_utils_unit utils_test.cpp) -target_link_libraries( - ${PROJECT_NAME}_utils_unit - PRIVATE GTest::GTest - GTest::Main - tesseract::tesseract_command_language - ${PROJECT_NAME}_core - ${PROJECT_NAME}_simple) -target_compile_options(${PROJECT_NAME}_utils_unit PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} - ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) -target_compile_definitions(${PROJECT_NAME}_utils_unit PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) -target_clang_tidy(${PROJECT_NAME}_utils_unit ENABLE ${TESSERACT_ENABLE_CLANG_TIDY}) -target_cxx_version(${PROJECT_NAME}_utils_unit PRIVATE VERSION ${TESSERACT_CXX_VERSION}) -target_code_coverage( - ${PROJECT_NAME}_utils_unit - PRIVATE - ALL - EXCLUDE ${COVERAGE_EXCLUDE} - ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE}) -add_gtest_discover_tests(${PROJECT_NAME}_utils_unit) -add_dependencies(${PROJECT_NAME}_utils_unit ${PROJECT_NAME}_core) -add_dependencies(run_tests ${PROJECT_NAME}_utils_unit) diff --git a/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp b/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp index 5159c79055e..ea0c4cbe6b6 100644 --- a/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp +++ b/tesseract_motion_planners/core/test/profile_dictionary_tests.cpp @@ -29,9 +29,18 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include -struct ProfileBase +struct ProfileBase : public tesseract_planning::Profile { + ProfileBase() : Profile(ProfileBase::getStaticKey()) {} + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey() { return std::type_index(typeid(ProfileBase)).hash_code(); } + int a{ 0 }; }; @@ -41,8 +50,16 @@ struct ProfileTest : public ProfileBase ProfileTest(int a) { this->a = a; } }; -struct ProfileBase2 +struct ProfileBase2 : public tesseract_planning::Profile { + ProfileBase2() : Profile(ProfileBase2::getStaticKey()) {} + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey() { return std::type_index(typeid(ProfileBase2)).hash_code(); } + int b{ 0 }; }; @@ -58,69 +75,69 @@ TEST(TesseractPlanningProfileDictionaryUnit, ProfileDictionaryTest) // NOLINT { ProfileDictionary profiles; - EXPECT_FALSE(profiles.hasProfileEntry("ns")); + EXPECT_FALSE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); - profiles.addProfile("ns", "key", std::make_shared()); + profiles.addProfile("ns", "key", std::make_shared()); - EXPECT_TRUE(profiles.hasProfileEntry("ns")); - EXPECT_TRUE(profiles.hasProfile("ns", "key")); + EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase::getStaticKey(), "ns")); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key")); - auto profile = profiles.getProfile("ns", "key"); + auto profile = getProfile("ns", "key", profiles); EXPECT_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"); + profiles.addProfile("ns", "key2", profile); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key2")); + auto profile2 = getProfile("ns", "key2", profiles); EXPECT_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"); + profiles.addProfile("ns", "key", std::make_shared(10)); + EXPECT_TRUE(profiles.hasProfile(ProfileBase::getStaticKey(), "ns", "key")); + auto profile_check = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check != nullptr); EXPECT_EQ(profile_check->a, 10); - auto profile_map = profiles.getProfileEntry("ns"); + auto profile_map = profiles.getProfileEntry(ProfileBase::getStaticKey(), "ns"); auto it = profile_map.find("key"); EXPECT_TRUE(it != profile_map.end()); - EXPECT_EQ(it->second->a, 10); + EXPECT_EQ(std::static_pointer_cast(it->second)->a, 10); - profiles.addProfile("ns", "key", std::make_shared(20)); - auto profile_check2 = profiles.getProfile("ns", "key"); + profiles.addProfile("ns", "key", std::make_shared(20)); + auto profile_check2 = getProfile("ns", "key", profiles); EXPECT_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.getProfileEntry(0, "DoesNotExist")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile("DoesNotExist", "key")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "DoesNotExist", "key")); // NOLINT // Request a profile that does not exist - EXPECT_ANY_THROW(profiles.getProfile("ns", "DoesNotExist")); // NOLINT + EXPECT_ANY_THROW(profiles.getProfile(ProfileBase::getStaticKey(), "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", std::make_shared(20))); // NOLINT // Check adding a empty key - EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "", std::make_shared(20))); // NOLINT // Check adding a nullptr profile - EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // NOLINT + EXPECT_ANY_THROW(profiles.addProfile("ns", "key", nullptr)); // 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"); + profiles.addProfile("ns", "key", std::make_shared(5)); + EXPECT_TRUE(profiles.hasProfileEntry(ProfileBase2::getStaticKey(), "ns")); + EXPECT_TRUE(profiles.hasProfile(ProfileBase2::getStaticKey(), "ns", "key")); + auto profile_check3 = getProfile("ns", "key", profiles); EXPECT_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"); + auto profile_check4 = getProfile("ns", "key", profiles); EXPECT_TRUE(profile_check4 != nullptr); EXPECT_EQ(profile_check4->a, 20); } diff --git a/tesseract_motion_planners/core/test/utils_test.cpp b/tesseract_motion_planners/core/test/utils_test.cpp deleted file mode 100644 index 45a664f8c3b..00000000000 --- a/tesseract_motion_planners/core/test/utils_test.cpp +++ /dev/null @@ -1,1715 +0,0 @@ -/** - * @file utils_test.cpp - * @brief - * - * @author Matthew Powelson - * @date June 15, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace tesseract_planning; -using namespace tesseract_environment; - -class TesseractPlanningUtilsUnit : public ::testing::Test -{ -protected: - Environment::Ptr env_; - - void SetUp() override - { - auto locator = std::make_shared(); - Environment::Ptr env = std::make_shared(); - tesseract_common::fs::path urdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.urdf")->getFilePath()); - tesseract_common::fs::path srdf_path( - locator->locateResource("package://tesseract_support/urdf/lbr_iiwa_14_r820.srdf")->getFilePath()); - EXPECT_TRUE(env->init(urdf_path, srdf_path, locator)); - env_ = env; - } -}; - -TEST_F(TesseractPlanningUtilsUnit, GenerateSeed) // NOLINT -{ - EXPECT_TRUE(true); -} - -TEST_F(TesseractPlanningUtilsUnit, GetProfileStringTest) // NOLINT -{ - std::string input_profile; - std::string planner_name = "Planner_1"; - std::string default_planner = "TEST_DEFAULT"; - - std::unordered_map remap; - remap["profile_1"] = "profile_1_remapped"; - PlannerProfileRemapping remapping; - remapping["Planner_2"] = remap; - - // Empty input profile - std::string output_profile = getProfileString(planner_name, input_profile, remapping); - EXPECT_EQ(output_profile, "DEFAULT"); - output_profile = getProfileString(planner_name, input_profile, remapping, default_planner); - EXPECT_EQ(output_profile, default_planner); - - // Planner name doesn't match - input_profile = "profile_1"; - output_profile = getProfileString(planner_name, input_profile, remapping, default_planner); - EXPECT_EQ(input_profile, output_profile); - - // Profile name doesn't match - input_profile = "doesnt_match"; - output_profile = getProfileString("Planner_2", input_profile, remapping, default_planner); - EXPECT_EQ(input_profile, output_profile); - - // Successful remap - input_profile = "profile_1"; - output_profile = getProfileString("Planner_2", input_profile, remapping, default_planner); - EXPECT_EQ(output_profile, "profile_1_remapped"); -} - -void checkProcessInterpolatedResults(const std::vector& contacts) -{ - for (const auto& c : contacts) - { - for (const auto& pair : c) - { - for (const auto& r : pair.second) - { - for (std::size_t j = 0; j < 2; ++j) - { - if (!(r.cc_time[j] < 0)) - { - if (tesseract_common::almostEqualRelativeAndAbs(r.cc_time[j], 0.0)) - EXPECT_EQ(r.cc_type[j], tesseract_collision::ContinuousCollisionType::CCType_Time0); - else if (tesseract_common::almostEqualRelativeAndAbs(r.cc_time[j], 1.0)) - EXPECT_EQ(r.cc_type[j], tesseract_collision::ContinuousCollisionType::CCType_Time1); - else - EXPECT_EQ(r.cc_type[j], tesseract_collision::ContinuousCollisionType::CCType_Between); - } - else - { - EXPECT_EQ(r.cc_type[j], tesseract_collision::ContinuousCollisionType::CCType_None); - } - } - - EXPECT_TRUE((r.cc_type[0] != tesseract_collision::ContinuousCollisionType::CCType_None || - r.cc_type[1] != tesseract_collision::ContinuousCollisionType::CCType_None)); - } - } - } -} - -/** - * @brief Verify that no contact results is at Time0 - * @param contacts The contacts to check - */ -void checkProcessInterpolatedResultsNoTime0(const tesseract_collision::ContactResultMap& contacts) -{ - for (const auto& pair : contacts) - { - for (const auto& r : pair.second) - { - EXPECT_NE(r.cc_type[0], tesseract_collision::ContinuousCollisionType::CCType_Time0); - EXPECT_NE(r.cc_type[1], tesseract_collision::ContinuousCollisionType::CCType_Time0); - } - } -} - -/** - * @brief Verify that no contact results is at Time1 - * @param contacts The contacts to check - */ -void checkProcessInterpolatedResultsNoTime1(const tesseract_collision::ContactResultMap& contacts) -{ - for (const auto& pair : contacts) - { - for (const auto& r : pair.second) - { - EXPECT_NE(r.cc_type[0], tesseract_collision::ContinuousCollisionType::CCType_Time1); - EXPECT_NE(r.cc_type[1], tesseract_collision::ContinuousCollisionType::CCType_Time1); - } - } -} - -/** - * @brief Verify that the results contain Time0 - * @param contacts The contacts to check - */ -bool hasProcessInterpolatedResultsTime0(const tesseract_collision::ContactResultMap& contacts) -{ - for (const auto& pair : contacts) - { - for (const auto& r : pair.second) - { - if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time0) - return true; - - if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time0) - return true; - } - } - return false; -} - -/** - * @brief Verify that the results contain Time1 - * @param contacts The contacts to check - */ -bool hasProcessInterpolatedResultsTime1(const tesseract_collision::ContactResultMap& contacts) -{ - for (const auto& pair : contacts) - { - for (const auto& r : pair.second) - { - if (r.cc_type[0] == tesseract_collision::ContinuousCollisionType::CCType_Time1) - return true; - - if (r.cc_type[1] == tesseract_collision::ContinuousCollisionType::CCType_Time1) - return true; - } - } - return false; -} - -/** @brief Get the total number of contacts */ -long getContactCount(const std::vector& contacts) -{ - long total{ 0 }; - for (const auto& c : contacts) - total += c.count(); - - return total; -} - -TEST_F(TesseractPlanningUtilsUnit, checkProgramUnit) // NOLINT -{ - // Add sphere to environment - tesseract_scene_graph::Link link_sphere("sphere_attached"); - - tesseract_scene_graph::Visual::Ptr visual = std::make_shared(); - visual->origin = Eigen::Isometry3d::Identity(); - visual->origin.translation() = Eigen::Vector3d(0.5, 0, 0.55); - visual->geometry = std::make_shared(0.15); - link_sphere.visual.push_back(visual); - - tesseract_scene_graph::Collision::Ptr collision = std::make_shared(); - collision->origin = visual->origin; - collision->geometry = visual->geometry; - link_sphere.collision.push_back(collision); - - tesseract_scene_graph::Joint joint_sphere("joint_sphere_attached"); - joint_sphere.parent_link_name = "base_link"; - joint_sphere.child_link_name = link_sphere.getName(); - joint_sphere.type = tesseract_scene_graph::JointType::FIXED; - - auto cmd = std::make_shared(link_sphere, joint_sphere); - - EXPECT_TRUE(env_->applyCommand(cmd)); - - // Set the robot initial state - std::vector joint_names; - joint_names.emplace_back("joint_a1"); - joint_names.emplace_back("joint_a2"); - joint_names.emplace_back("joint_a3"); - joint_names.emplace_back("joint_a4"); - joint_names.emplace_back("joint_a5"); - joint_names.emplace_back("joint_a6"); - joint_names.emplace_back("joint_a7"); - - Eigen::VectorXd joint_start_pos(7); - joint_start_pos(0) = -0.4; - joint_start_pos(1) = 0.2762; - joint_start_pos(2) = 0.0; - joint_start_pos(3) = -1.3348; - joint_start_pos(4) = 0.0; - joint_start_pos(5) = 1.4959; - joint_start_pos(6) = 0.0; - - Eigen::VectorXd joint_end_pos(7); - joint_end_pos(0) = 0.4; - joint_end_pos(1) = 0.2762; - joint_end_pos(2) = 0.0; - joint_end_pos(3) = -1.3348; - joint_end_pos(4) = 0.0; - joint_end_pos(5) = 1.4959; - joint_end_pos(6) = 0.0; - - Eigen::VectorXd joint_pos_collision(7); - joint_pos_collision(0) = 0.0; - joint_pos_collision(1) = 0.2762; - joint_pos_collision(2) = 0.0; - joint_pos_collision(3) = -1.3348; - joint_pos_collision(4) = 0.0; - joint_pos_collision(5) = 1.4959; - joint_pos_collision(6) = 0.0; - - using tesseract_planning::CompositeInstruction; - using tesseract_planning::contactCheckProgram; - using tesseract_planning::MoveInstruction; - using tesseract_planning::MoveInstructionType; - using tesseract_planning::StateWaypoint; - using tesseract_planning::StateWaypointPoly; - - // Only intermediat states are in collision - tesseract_common::TrajArray traj(5, joint_start_pos.size()); - for (int i = 0; i < joint_start_pos.size(); ++i) - traj.col(i) = Eigen::VectorXd::LinSpaced(5, joint_start_pos(i), joint_end_pos(i)); - - CompositeInstruction traj_ci; - for (long r = 0; r < traj.rows(); ++r) - { - StateWaypointPoly swp{ StateWaypoint(joint_names, traj.row(r)) }; - traj_ci.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); - } - - // Only start state is not in collision - tesseract_common::TrajArray traj2(3, joint_start_pos.size()); - for (int i = 0; i < joint_start_pos.size(); ++i) - traj2.col(i) = Eigen::VectorXd::LinSpaced(3, joint_start_pos(i), joint_pos_collision(i)); - - CompositeInstruction traj2_ci; - for (long r = 0; r < traj2.rows(); ++r) - { - StateWaypointPoly swp{ StateWaypoint(joint_names, traj2.row(r)) }; - traj2_ci.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); - } - - // Only start state is not in collision - tesseract_common::TrajArray traj3(3, joint_start_pos.size()); - for (int i = 0; i < joint_start_pos.size(); ++i) - traj3.col(i) = Eigen::VectorXd::LinSpaced(3, joint_pos_collision(i), joint_end_pos(i)); - - CompositeInstruction traj3_ci; - for (long r = 0; r < traj3.rows(); ++r) - { - StateWaypointPoly swp{ StateWaypoint(joint_names, traj3.row(r)) }; - traj3_ci.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); - } - - // Only two states - tesseract_common::TrajArray traj4(2, joint_start_pos.size()); - traj4.row(0) = joint_pos_collision; - traj4.row(1) = joint_end_pos; - - CompositeInstruction traj4_ci; - for (long r = 0; r < traj4.rows(); ++r) - { - StateWaypointPoly swp{ StateWaypoint(joint_names, traj4.row(r)) }; - traj4_ci.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); - } - - auto discrete_manager = env_->getDiscreteContactManager(); - auto continuous_manager = env_->getContinuousContactManager(); - auto state_solver = env_->getStateSolver(); - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(contacts.at(4).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(contacts.at(4).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size())); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::DISCRETE && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResults(contacts); - } - - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 2); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 2); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResults(contacts); - } - - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - std::vector contacts; - - CompositeInstruction ci; - StateWaypointPoly swp{ StateWaypoint(joint_names, joint_start_pos) }; - ci.appendMoveInstruction(MoveInstruction(swp, MoveInstructionType::FREESPACE)); - - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj4_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj4_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj4_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj4_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj4_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj4_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 2); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(6)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj4_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj4_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - config.longest_valid_segment_length = std::numeric_limits::max(); - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 2); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 2); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(285)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(125)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(160)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(285)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(123)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(160)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(285)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(125)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(1))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(158)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(285)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(2))); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(3))); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(123)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(158)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(1))); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_DISCRETE && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime0(contacts.at(0))); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 9 instead of 6 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(9)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(5)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - /**< @todo we are getting 9 instead of 6 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(7)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(2)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(0))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(getContactCount(contacts), static_cast(3)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 7 instead of 4 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(7)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime0(contacts.at(1)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(getContactCount(contacts), static_cast(5)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(2)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::CONTINUOUS && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - } - - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 9 instead of 6 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(9)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(4)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(5)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - /**< @todo we are getting 9 instead of 6 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(7)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(2)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - EXPECT_TRUE(hasProcessInterpolatedResultsTime1(contacts.at(0))); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(getContactCount(contacts), static_cast(3)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 7 instead of 4 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(7)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime0(contacts.at(1)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(getContactCount(contacts), static_cast(5)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(2)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 2)); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.longest_valid_segment_length = std::numeric_limits::max(); - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - } - - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - config.longest_valid_segment_length = std::numeric_limits::max(); - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 1); - EXPECT_EQ(getContactCount(contacts), static_cast(1)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 288 instead of 285 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(288)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(125)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(161)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_END - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_END; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 9 instead of 6 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(288)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(123)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(getContactCount(contacts), static_cast(161)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(0)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::ALL_EXCEPT_START - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::ALL_EXCEPT_START; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - /**< @todo we are getting 7 instead of 4 because duplicate contacts at states */ - EXPECT_EQ(getContactCount(contacts), static_cast(288)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(125)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(159)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::INTERMEDIATE_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::INTERMEDIATE_ONLY; - std::vector contacts; - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(contacts.at(2).size(), 3); - EXPECT_EQ(contacts.at(3).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(288)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(3)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(123)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), static_cast(traj3_ci.size() - 1)); - EXPECT_EQ(contacts.at(0).size(), 3); - EXPECT_EQ(contacts.at(1).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(159)); - checkProcessInterpolatedResultsNoTime0(contacts.at(0)); - checkProcessInterpolatedResultsNoTime1(contacts.at(1)); - checkProcessInterpolatedResults(contacts); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::END_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::END_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - } - - { // CollisionEvaluatorType::LVS_CONTINUOUS && CollisionCheckProgramType::START_ONLY - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - config.check_program_mode = tesseract_collision::CollisionCheckProgramType::START_ONLY; - std::vector contacts; - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_FALSE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj2_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 0); - EXPECT_EQ(getContactCount(contacts), static_cast(0)); - - contacts.clear(); - EXPECT_TRUE(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj3_ci, config)); - EXPECT_EQ(contacts.size(), 1); - EXPECT_EQ(contacts.at(0).size(), 2); - EXPECT_EQ(getContactCount(contacts), static_cast(2)); - } - - // Failures - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - std::vector contacts; - // NOLINTNEXTLINE - EXPECT_ANY_THROW(contactCheckProgram(contacts, *discrete_manager, *state_solver, traj_ci, config)); - } - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - std::vector contacts; - // NOLINTNEXTLINE - EXPECT_ANY_THROW(contactCheckProgram( - contacts, *discrete_manager, *state_solver, tesseract_planning::CompositeInstruction(), config)); - } - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - std::vector contacts; - // NOLINTNEXTLINE - EXPECT_ANY_THROW(contactCheckProgram(contacts, *continuous_manager, *state_solver, traj_ci, config)); - } - { - tesseract_collision::CollisionCheckConfig config; - config.type = tesseract_collision::CollisionEvaluatorType::CONTINUOUS; - std::vector contacts; - // NOLINTNEXTLINE - EXPECT_ANY_THROW(contactCheckProgram( - contacts, *continuous_manager, *state_solver, tesseract_planning::CompositeInstruction(), config)); - } -} -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index e7626c99338..5eafd22e6a8 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -11,6 +11,7 @@ add_library( src/serialize.cpp src/deserialize.cpp src/descartes_utils.cpp + src/profile/descartes_profile.cpp src/profile/descartes_default_plan_profile.cpp) target_link_libraries( ${PROJECT_NAME}_descartes 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 7df99ed6e32..ade84d6c14d 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 @@ -259,8 +259,8 @@ DescartesMotionPlanner::createProblem(const PlannerRequest& request) return prob; } - prob->env_state = request.env_state; prob->env = request.env; + prob->env_state = request.env->getState(); std::vector joint_names = prob->manip->getJointNames(); @@ -289,12 +289,12 @@ 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); + auto cur_plan_profile = + getProfile>(name_, + plan_instruction.getProfile(name_), + *request.profiles, + std::make_shared>()); + if (!cur_plan_profile) throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h index 63460a66659..1bdde8ee7d4 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h @@ -47,11 +47,6 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile using ConstPtr = std::shared_ptr>; DescartesDefaultPlanProfile() = default; - ~DescartesDefaultPlanProfile() override = default; - DescartesDefaultPlanProfile(const DescartesDefaultPlanProfile&) = default; - DescartesDefaultPlanProfile& operator=(const DescartesDefaultPlanProfile&) = default; - DescartesDefaultPlanProfile(DescartesDefaultPlanProfile&&) noexcept = default; - DescartesDefaultPlanProfile& operator=(DescartesDefaultPlanProfile&&) noexcept = default; DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); PoseSamplerFn target_pose_sampler = sampleFixed; @@ -103,10 +98,18 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile int index) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; using DescartesDefaultPlanProfileF = DescartesDefaultPlanProfile; using DescartesDefaultPlanProfileD = DescartesDefaultPlanProfile; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h index e6a02fe316e..8f90e755ff3 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_profile.h @@ -29,11 +29,13 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include namespace tinyxml2 @@ -45,18 +47,19 @@ class XMLDocument; namespace tesseract_planning { template -class DescartesPlanProfile +class DescartesPlanProfile : public Profile { public: using Ptr = std::shared_ptr>; using ConstPtr = std::shared_ptr>; - DescartesPlanProfile() = default; - virtual ~DescartesPlanProfile() = default; - DescartesPlanProfile(const DescartesPlanProfile&) = default; - DescartesPlanProfile& operator=(const DescartesPlanProfile&) = default; - DescartesPlanProfile(DescartesPlanProfile&&) noexcept = default; - DescartesPlanProfile& operator=(DescartesPlanProfile&&) noexcept = default; + DescartesPlanProfile() : Profile(DescartesPlanProfile::getStaticKey()) {} + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey() { return std::type_index(typeid(DescartesPlanProfile)).hash_code(); } virtual void apply(DescartesProblem& prob, const Eigen::Isometry3d& cartesian_waypoint, @@ -71,6 +74,11 @@ class DescartesPlanProfile int index) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int); // NOLINT }; /** @todo Currently descartes does not have support of composite profile everything is handled by the plan profile */ diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp index af0b93125e8..70d073c23bd 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_default_plan_profile.cpp @@ -24,6 +24,10 @@ * limitations under the License. */ #include +#include +#include +#include +#include namespace tesseract_planning { @@ -31,4 +35,30 @@ namespace tesseract_planning template class DescartesDefaultPlanProfile; template class DescartesDefaultPlanProfile; +template +template +void DescartesDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesPlanProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(target_pose_sampler); + // ar& BOOST_SERIALIZATION_NVP(edge_evaluator); + // ar& BOOST_SERIALIZATION_NVP(state_evaluator); + // ar& BOOST_SERIALIZATION_NVP(vertex_evaluator); + ar& BOOST_SERIALIZATION_NVP(allow_collision); + ar& BOOST_SERIALIZATION_NVP(enable_collision); + ar& BOOST_SERIALIZATION_NVP(vertex_collision_check_config); + ar& BOOST_SERIALIZATION_NVP(enable_edge_collision); + ar& BOOST_SERIALIZATION_NVP(edge_collision_check_config); + ar& BOOST_SERIALIZATION_NVP(use_redundant_joint_solutions); + ar& BOOST_SERIALIZATION_NVP(num_threads); + ar& BOOST_SERIALIZATION_NVP(debug); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesDefaultPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesDefaultPlanProfile) diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp new file mode 100644 index 00000000000..0eeffa0a126 --- /dev/null +++ b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp @@ -0,0 +1,45 @@ +/** + * @file descartes_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include + +namespace tesseract_planning +{ +template +template +void DescartesPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index b8dc02bd2f1..9debf0705e0 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -109,13 +109,6 @@ TEST(TesseractPlanningDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -137,15 +130,14 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); @@ -155,7 +147,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; PlannerResponse single_planner_response = single_descartes_planner.solve(request); @@ -221,13 +212,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.20, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -249,8 +233,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -261,7 +244,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); @@ -271,7 +254,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; auto problem = single_descartes_planner.createProblem(request); @@ -324,13 +306,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - tesseract_kinematics::KinematicGroup::Ptr kin_group = - env_->getKinematicGroup(manip.manipulator, manip.manipulator_ik_solver); - auto cur_state = env_->getState(); - // Specify a start waypoint CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.8, -.10, 0.8) * Eigen::Quaterniond(0, 0, -1.0, 0)) }; @@ -352,7 +327,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 2); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 2); // Create Profiles auto plan_profile = std::make_shared(); @@ -364,13 +339,12 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile>(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planning Request PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; // Create Planner diff --git a/tesseract_motion_planners/examples/chain_example.cpp b/tesseract_motion_planners/examples/chain_example.cpp index 019612e281c..565f1c42806 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 @@ -145,15 +146,17 @@ 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", 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 c235fb9a53e..d8f9993dc3f 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 @@ -116,15 +117,17 @@ 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); // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_plan_profile); - profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "DEFAULT", trajopt_composite_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "DEFAULT", ompl_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 0554bc7565f..5548a23ae8c 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 @@ -92,7 +93,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)) }; @@ -116,14 +117,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"); @@ -144,7 +145,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"); @@ -153,7 +154,7 @@ int main(int /*argc*/, char** /*argv*/) transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); @@ -173,7 +174,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"); @@ -182,7 +183,7 @@ int main(int /*argc*/, char** /*argv*/) transition_from_start.setDescription("transition_from_start"); transition_from_start.appendMoveInstruction(plan_f1); - CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED); + CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED); transitions.setDescription("transitions"); transitions.push_back(transition_from_start); transitions.push_back(transition_from_end); @@ -201,7 +202,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"); @@ -218,15 +219,17 @@ 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; diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt index cac76bc1ec2..ae481465b31 100644 --- a/tesseract_motion_planners/ompl/CMakeLists.txt +++ b/tesseract_motion_planners/ompl/CMakeLists.txt @@ -10,6 +10,7 @@ set(OMPL_SRC src/weighted_real_vector_state_sampler.cpp src/ompl_planner_configurator.cpp src/ompl_problem.cpp + src/profile/ompl_profile.cpp src/profile/ompl_default_plan_profile.cpp src/utils.cpp src/state_collision_validator.cpp diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h index 87cf2a1421d..c4f3dcfe879 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_planner_configurator.h @@ -35,6 +35,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -87,6 +89,11 @@ struct OMPLPlannerConfigurator virtual OMPLPlannerType getType() const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct SBLConfigurator : public OMPLPlannerConfigurator @@ -109,6 +116,11 @@ struct SBLConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct ESTConfigurator : public OMPLPlannerConfigurator @@ -134,6 +146,11 @@ struct ESTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct LBKPIECE1Configurator : public OMPLPlannerConfigurator @@ -162,6 +179,11 @@ struct LBKPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct BKPIECE1Configurator : public OMPLPlannerConfigurator @@ -193,6 +215,11 @@ struct BKPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct KPIECE1Configurator : public OMPLPlannerConfigurator @@ -227,6 +254,11 @@ struct KPIECE1Configurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct BiTRRTConfigurator : public OMPLPlannerConfigurator @@ -265,6 +297,11 @@ struct BiTRRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTConfigurator : public OMPLPlannerConfigurator @@ -290,6 +327,11 @@ struct RRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTConnectConfigurator : public OMPLPlannerConfigurator @@ -312,6 +354,11 @@ struct RRTConnectConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct RRTstarConfigurator : public OMPLPlannerConfigurator @@ -340,6 +387,11 @@ struct RRTstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct TRRTConfigurator : public OMPLPlannerConfigurator @@ -377,6 +429,11 @@ struct TRRTConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct PRMConfigurator : public OMPLPlannerConfigurator @@ -399,6 +456,11 @@ struct PRMConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct PRMstarConfigurator : public OMPLPlannerConfigurator @@ -418,6 +480,11 @@ struct PRMstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct LazyPRMstarConfigurator : public OMPLPlannerConfigurator @@ -437,6 +504,11 @@ struct LazyPRMstarConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; struct SPARSConfigurator : public OMPLPlannerConfigurator @@ -468,8 +540,29 @@ struct SPARSConfigurator : public OMPLPlannerConfigurator /** @brief Serialize planner to xml */ tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlannerConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SBLConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ESTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::LBKPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::BKPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::KPIECE1Configurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::BiTRRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTConnectConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RRTstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TRRTConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::PRMConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::PRMstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::LazyPRMstarConfigurator) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SPARSConfigurator) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_PLANNER_CONFIGURATOR_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h index b65cfbef016..54d3469cf8a 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h @@ -62,11 +62,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile using ConstPtr = std::shared_ptr; OMPLDefaultPlanProfile(); - ~OMPLDefaultPlanProfile() override = default; - OMPLDefaultPlanProfile(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile& operator=(const OMPLDefaultPlanProfile&) = default; - OMPLDefaultPlanProfile(OMPLDefaultPlanProfile&&) noexcept = default; - OMPLDefaultPlanProfile& operator=(OMPLDefaultPlanProfile&&) noexcept = default; OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); /** @brief The OMPL state space to use when planning */ @@ -160,6 +155,13 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile void processMotionValidator(OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const; void processOptimizationObjective(OMPLProblem& prob) const; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h index 1e64f17c4af..1d5822238a7 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tinyxml2 { @@ -45,20 +46,19 @@ class XMLDocument; namespace tesseract_planning { struct OMPLProblem; -class OMPLPlanProfile +class OMPLPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - OMPLPlanProfile() = default; - virtual ~OMPLPlanProfile() = default; - OMPLPlanProfile(const OMPLPlanProfile&) = default; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = default; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = default; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = default; + OMPLPlanProfile(); - OMPLPlanProfile(const tinyxml2::XMLElement& xml_element); + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void setup(OMPLProblem& prob) const = 0; @@ -91,10 +91,17 @@ class OMPLPlanProfile int index) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; /** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */ } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_OMPL_OMPL_PROFILE_H diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 7bc73244994..c35aaa89b11 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -333,11 +333,9 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, 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()); + auto cur_plan_profile = getProfile( + name_, end_instruction.getProfile(name_), *request.profiles, std::make_shared()); + if (!cur_plan_profile) throw std::runtime_error("OMPLMotionPlanner: Invalid profile"); @@ -347,10 +345,10 @@ OMPLMotionPlanner::createSubProblem(const PlannerRequest& request, config.end_uuid = end_instruction.getUUID(); config.problem = std::make_shared(); config.problem->env = request.env; - config.problem->env_state = request.env_state; + config.problem->env_state = request.env->getState(); config.problem->manip = manip; config.problem->contact_checker = request.env->getDiscreteContactManager(); - config.problem->contact_checker->setCollisionObjectsTransform(request.env_state.link_transforms); + config.problem->contact_checker->setCollisionObjectsTransform(config.problem->env_state.link_transforms); config.problem->contact_checker->setActiveCollisionObjects(active_link_names); cur_plan_profile->setup(*config.problem); diff --git a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp index d23c36466d8..9488d20ce2a 100644 --- a/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_planner_configurator.cpp @@ -44,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -52,6 +53,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { +template +void OMPLPlannerConfigurator::serialize(Archive& /*ar*/, const unsigned int /*version*/) +{ +} + SBLConfigurator::SBLConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* sbl_element = xml_element.FirstChildElement("SBL"); @@ -91,6 +97,13 @@ tinyxml2::XMLElement* SBLConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void SBLConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); +} + ESTConfigurator::ESTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* est_element = xml_element.FirstChildElement("EST"); @@ -151,6 +164,14 @@ tinyxml2::XMLElement* ESTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void ESTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); +} + LBKPIECE1Configurator::LBKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* lbkpiece1_element = xml_element.FirstChildElement("LBKPIECE1"); @@ -231,6 +252,15 @@ tinyxml2::XMLElement* LBKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) c return ompl_xml; } +template +void LBKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + BKPIECE1Configurator::BKPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* bkpiece1_element = xml_element.FirstChildElement("BKPIECE1"); @@ -333,6 +363,16 @@ tinyxml2::XMLElement* BKPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) co return ompl_xml; } +template +void BKPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(failed_expansion_score_factor); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + KPIECE1Configurator::KPIECE1Configurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* kpiece1_element = xml_element.FirstChildElement("KPIECE1"); @@ -454,6 +494,17 @@ tinyxml2::XMLElement* KPIECE1Configurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void KPIECE1Configurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(border_fraction); + ar& BOOST_SERIALIZATION_NVP(failed_expansion_score_factor); + ar& BOOST_SERIALIZATION_NVP(min_valid_path_fraction); +} + BiTRRTConfigurator::BiTRRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* bitrrt_element = xml_element.FirstChildElement("BiTRRT"); @@ -593,6 +644,18 @@ tinyxml2::XMLElement* BiTRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) cons return ompl_xml; } +template +void BiTRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(temp_change_factor); + ar& BOOST_SERIALIZATION_NVP(cost_threshold); + ar& BOOST_SERIALIZATION_NVP(init_temperature); + ar& BOOST_SERIALIZATION_NVP(frontier_threshold); + ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); +} + RRTConfigurator::RRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_element = xml_element.FirstChildElement("RRT"); @@ -653,6 +716,14 @@ tinyxml2::XMLElement* RRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void RRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); +} + RRTConnectConfigurator::RRTConnectConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_connect_element = xml_element.FirstChildElement("RRTConnect"); @@ -694,6 +765,13 @@ tinyxml2::XMLElement* RRTConnectConfigurator::toXML(tinyxml2::XMLDocument& doc) return ompl_xml; } +template +void RRTConnectConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); +} + RRTstarConfigurator::RRTstarConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* rrt_star_element = xml_element.FirstChildElement("RRTstar"); @@ -769,6 +847,15 @@ tinyxml2::XMLElement* RRTstarConfigurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void RRTstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(delay_collision_checking); +} + TRRTConfigurator::TRRTConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* trrt_element = xml_element.FirstChildElement("TRRT"); @@ -905,6 +992,18 @@ tinyxml2::XMLElement* TRRTConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void TRRTConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(range); + ar& BOOST_SERIALIZATION_NVP(goal_bias); + ar& BOOST_SERIALIZATION_NVP(temp_change_factor); + ar& BOOST_SERIALIZATION_NVP(init_temperature); + ar& BOOST_SERIALIZATION_NVP(frontier_threshold); + ar& BOOST_SERIALIZATION_NVP(frontier_node_ratio); +} + PRMConfigurator::PRMConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* prm_element = xml_element.FirstChildElement("PRM"); @@ -946,6 +1045,13 @@ tinyxml2::XMLElement* PRMConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } +template +void PRMConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(max_nearest_neighbors); +} + PRMstarConfigurator::PRMstarConfigurator(const tinyxml2::XMLElement&) {} ompl::base::PlannerPtr PRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const @@ -962,6 +1068,12 @@ tinyxml2::XMLElement* PRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) con return ompl_xml; } +template +void PRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); +} + LazyPRMstarConfigurator::LazyPRMstarConfigurator(const tinyxml2::XMLElement&) {} ompl::base::PlannerPtr LazyPRMstarConfigurator::create(ompl::base::SpaceInformationPtr si) const @@ -978,6 +1090,12 @@ tinyxml2::XMLElement* LazyPRMstarConfigurator::toXML(tinyxml2::XMLDocument& doc) return ompl_xml; } +template +void LazyPRMstarConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); +} + SPARSConfigurator::SPARSConfigurator(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* spars_element = xml_element.FirstChildElement("SPARS"); @@ -1075,4 +1193,47 @@ tinyxml2::XMLElement* SPARSConfigurator::toXML(tinyxml2::XMLDocument& doc) const return ompl_xml; } + +template +void SPARSConfigurator::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlannerConfigurator); + ar& BOOST_SERIALIZATION_NVP(max_failures); + ar& BOOST_SERIALIZATION_NVP(dense_delta_fraction); + ar& BOOST_SERIALIZATION_NVP(sparse_delta_fraction); + ar& BOOST_SERIALIZATION_NVP(stretch_factor); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLPlannerConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SBLConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ESTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::LBKPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::BKPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::KPIECE1Configurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::BiTRRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTConnectConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RRTstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TRRTConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::PRMConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::PRMstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::LazyPRMstarConfigurator) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SPARSConfigurator) + +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLPlannerConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SBLConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ESTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::LBKPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::BKPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::KPIECE1Configurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::BiTRRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTConnectConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RRTstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TRRTConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::PRMConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::PRMstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::LazyPRMstarConfigurator) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SPARSConfigurator) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp index 9002d1b27c0..a3f164a3b09 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp @@ -33,6 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -50,6 +54,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include namespace tesseract_planning @@ -777,4 +782,25 @@ void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) con } } +template +void OMPLDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_space); + ar& BOOST_SERIALIZATION_NVP(planning_time); + ar& BOOST_SERIALIZATION_NVP(max_solutions); + ar& BOOST_SERIALIZATION_NVP(simplify); + ar& BOOST_SERIALIZATION_NVP(optimize); + ar& BOOST_SERIALIZATION_NVP(planners); + ar& BOOST_SERIALIZATION_NVP(collision_check_config); + // ar& BOOST_SERIALIZATION_NVP(state_sampler_allocator); + // ar& BOOST_SERIALIZATION_NVP(optimization_objective_allocator); + // ar& BOOST_SERIALIZATION_NVP(svc_allocator); + // ar& BOOST_SERIALIZATION_NVP(mv_allocator); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLDefaultPlanProfile) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp new file mode 100644 index 00000000000..2763116d0bd --- /dev/null +++ b/tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp @@ -0,0 +1,48 @@ +/** + * @file ompl_profile.cpp + * @brief Tesseract OMPL profile + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +OMPLPlanProfile::OMPLPlanProfile() : Profile(OMPLPlanProfile::getStaticKey()) {} + +std::size_t OMPLPlanProfile::getStaticKey() { return std::type_index(typeid(OMPLPlanProfile)).hash_code(); } + +template +void OMPLPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::OMPLPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::OMPLPlanProfile) diff --git a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 09e771152b2..1fa5b179818 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -198,7 +198,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto joint_group = env->getJointGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint JointWaypointPoly wp1{ JointWaypoint( @@ -225,7 +224,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f2); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -242,13 +241,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve @@ -295,7 +293,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a new seed - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Update Configuration request.instructions = interpolated_program; @@ -328,7 +326,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a new seed - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Update Configuration request.instructions = interpolated_program; @@ -364,7 +362,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint JointWaypointPoly wp1{ JointWaypoint( @@ -389,7 +386,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -406,13 +403,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve @@ -459,7 +455,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT // Step 3: Create ompl planner config and populate it auto kin_group = env->getKinematicGroup(manip.manipulator); - auto cur_state = env->getState(); // Specify a start waypoint auto start_jv = Eigen::Map(start_state.data(), static_cast(start_state.size())); @@ -484,7 +479,7 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -501,13 +496,12 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT // Profile Dictionary auto profiles = std::make_shared(); - profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); // Create Planner Request PlannerRequest request; request.instructions = interpolated_program; request.env = env; - request.env_state = cur_state; request.profiles = profiles; // Create Planner and solve diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 41d30a898d4..838cfbd4546 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -2,8 +2,11 @@ add_library( ${PROJECT_NAME}_simple src/interpolation.cpp src/simple_motion_planner.cpp - src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp + src/profile/simple_planner_profile.cpp + src/profile/simple_planner_lvs_plan_profile.cpp + src/profile/simple_planner_lvs_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_assign_plan_profile.cpp + src/profile/simple_planner_fixed_size_assign_no_ik_plan_profile.cpp src/profile/simple_planner_fixed_size_plan_profile.cpp src/profile/simple_planner_lvs_assign_no_ik_plan_profile.cpp src/profile/simple_planner_lvs_assign_plan_profile.cpp diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h index db73c3557c4..205b68c2297 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h @@ -507,7 +507,6 @@ std::array getClosestJointSolution(const KinematicGroupInstr /** @brief Provided for backwards compatibility */ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions, - const tesseract_scene_graph::SceneState& current_state, const std::shared_ptr& env, double state_longest_valid_segment_length = 5 * M_PI / 180, double translation_longest_valid_segment_length = 0.15, diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h index e3bba2eea8f..70e6c29d0ce 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h @@ -56,8 +56,15 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile /** @brief The number of steps to use for linear instruction */ int linear_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_ASSIGN_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h index 07d0d460211..cbb48096a68 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h @@ -55,8 +55,15 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile /** @brief The number of steps to use for linear instruction */ int linear_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerFixedSizePlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_FIXED_SIZE_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h index 174d8f63964..0cdf7d4396a 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h @@ -78,8 +78,15 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_LVS_NO_IK_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h index 7dde313bd0b..92a52006b45 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h @@ -77,8 +77,15 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile /** @brief The maximum number of steps for the plan */ int max_steps; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerLVSPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_LVS_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h index b55d21597ec..07d969a42c9 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h @@ -36,6 +36,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_planning { @@ -43,18 +44,19 @@ namespace tesseract_planning * @brief Plan Profile for the simple planner. It defines some functions that handle each of the waypoint cases. The * planner then simply loops over all of the plan instructions and calls the correct function */ -class SimplePlannerPlanProfile +class SimplePlannerPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerPlanProfile() = default; - virtual ~SimplePlannerPlanProfile() = default; - SimplePlannerPlanProfile(const SimplePlannerPlanProfile&) = default; - SimplePlannerPlanProfile& operator=(const SimplePlannerPlanProfile&) = default; - SimplePlannerPlanProfile(SimplePlannerPlanProfile&&) noexcept = default; - SimplePlannerPlanProfile& operator=(SimplePlannerPlanProfile&&) noexcept = default; + SimplePlannerPlanProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** * @brief Generate a seed for the provided base_instruction @@ -74,24 +76,36 @@ class SimplePlannerPlanProfile const InstructionPoly& next_instruction, const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class SimplePlannerCompositeProfile +class SimplePlannerCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - SimplePlannerCompositeProfile() = default; - virtual ~SimplePlannerCompositeProfile() = default; - SimplePlannerCompositeProfile(const SimplePlannerCompositeProfile&) = default; - SimplePlannerCompositeProfile& operator=(const SimplePlannerCompositeProfile&) = default; - SimplePlannerCompositeProfile(SimplePlannerCompositeProfile&&) noexcept = default; - SimplePlannerCompositeProfile& operator=(SimplePlannerCompositeProfile&&) noexcept = default; + SimplePlannerCompositeProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); // This contains functions for composite processing. Get start for example +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::SimplePlannerPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_SIMPLE_PROFILE_H diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h index 063023398bd..2cfa0bc0e48 100644 --- a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h +++ b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/simple_motion_planner.h @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_planning { @@ -66,9 +67,10 @@ class SimpleMotionPlanner : public MotionPlanner std::unique_ptr clone() const override; protected: - CompositeInstruction processCompositeInstruction(const CompositeInstruction& instructions, - MoveInstructionPoly& prev_instruction, + CompositeInstruction processCompositeInstruction(MoveInstructionPoly& prev_instruction, MoveInstructionPoly& prev_seed, + const CompositeInstruction& instructions, + const tesseract_scene_graph::SceneState& start_state, const PlannerRequest& request) const; }; diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 09fd2f9bc23..7766a23d7fd 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -55,7 +55,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { - JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info) @@ -1312,7 +1311,6 @@ std::array getClosestJointSolution(const KinematicGroupInstr } CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& instructions, - const tesseract_scene_graph::SceneState& current_state, const std::shared_ptr& env, double state_longest_valid_segment_length, double translation_longest_valid_segment_length, @@ -1322,7 +1320,6 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Fill out request and response PlannerRequest request; request.instructions = instructions; - request.env_state = current_state; request.env = env; // Set up planner @@ -1335,11 +1332,10 @@ CompositeInstruction generateInterpolatedProgram(const CompositeInstruction& ins // Create profile dictionary auto profiles = std::make_shared(); - profiles->addProfile(planner.getName(), instructions.getProfile(), profile); + profiles->addProfile(planner.getName(), instructions.getProfile(), profile); auto flat = instructions.flatten(&moveFilter); for (const auto& i : flat) - profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + profiles->addProfile(planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = profiles; diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp index 70539b3b13c..2a630b34891 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp @@ -32,11 +32,14 @@ #include #include -#include -#include +#include +#include #include +#include +#include + namespace tesseract_planning { SimplePlannerFixedSizeAssignPlanProfile::SimplePlannerFixedSizeAssignPlanProfile(int freespace_steps, int linear_steps) @@ -129,4 +132,16 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre return getInterpolatedInstructions(base.manip->getJointNames(), states, base.instruction); } +template +void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(freespace_steps); + ar& BOOST_SERIALIZATION_NVP(linear_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp index e47383047a5..3b45c2c06f6 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp @@ -33,6 +33,9 @@ #include #include +#include +#include + namespace tesseract_planning { SimplePlannerFixedSizePlanProfile::SimplePlannerFixedSizePlanProfile(int freespace_steps, int linear_steps) @@ -63,4 +66,16 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState()); } +template +void SimplePlannerFixedSizePlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(freespace_steps); + ar& BOOST_SERIALIZATION_NVP(linear_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerFixedSizePlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerFixedSizePlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp index ba12afaf669..d11487843b0 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp @@ -33,6 +33,9 @@ #include #include +#include +#include + namespace tesseract_planning { SimplePlannerLVSNoIKPlanProfile::SimplePlannerLVSNoIKPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,19 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru env->getState()); } +template +void SimplePlannerLVSNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp index 457838c0980..82f664368df 100644 --- a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp @@ -33,6 +33,9 @@ #include #include +#include +#include + namespace tesseract_planning { SimplePlannerLVSPlanProfile::SimplePlannerLVSPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,19 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio env->getState()); } +template +void SimplePlannerLVSPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); + ar& BOOST_SERIALIZATION_NVP(state_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(translation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(rotation_longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(min_steps); + ar& BOOST_SERIALIZATION_NVP(max_steps); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerLVSPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerLVSPlanProfile) diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp new file mode 100644 index 00000000000..fa055489a01 --- /dev/null +++ b/tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp @@ -0,0 +1,67 @@ +/** + * @file simple_planner_profile.cpp + * @brief + * + * @author Matthew Powelson + * @date July 23, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include + +namespace tesseract_planning +{ +SimplePlannerPlanProfile::SimplePlannerPlanProfile() : Profile(SimplePlannerPlanProfile::getStaticKey()) {} + +std::size_t SimplePlannerPlanProfile::getStaticKey() +{ + return std::type_index(typeid(SimplePlannerPlanProfile)).hash_code(); +} + +template +void SimplePlannerPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +SimplePlannerCompositeProfile::SimplePlannerCompositeProfile() : Profile(SimplePlannerCompositeProfile::getStaticKey()) +{ +} + +std::size_t SimplePlannerCompositeProfile::getStaticKey() +{ + return std::type_index(typeid(SimplePlannerCompositeProfile)).hash_code(); +} + +template +void SimplePlannerCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SimplePlannerCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SimplePlannerCompositeProfile) diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index fac3d195891..bdb8356a8ec 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -85,11 +85,13 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const // Assume all the plan instructions have the same manipulator as the composite const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; // Initialize tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); + // Start State + tesseract_scene_graph::SceneState start_state = request.env->getState(); + // Create seed CompositeInstruction seed; @@ -101,8 +103,8 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const { MoveInstructionPoly start_instruction_copy = null_instruction; MoveInstructionPoly start_instruction_seed_copy = null_instruction; - seed = - processCompositeInstruction(request.instructions, start_instruction_copy, start_instruction_seed_copy, request); + seed = processCompositeInstruction( + start_instruction_copy, start_instruction_seed_copy, request.instructions, start_state, request); } catch (std::exception& e) { @@ -148,10 +150,12 @@ PlannerResponse SimpleMotionPlanner::solve(const PlannerRequest& request) const return response; } -CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const CompositeInstruction& instructions, - MoveInstructionPoly& prev_instruction, - MoveInstructionPoly& prev_seed, - const PlannerRequest& request) const +CompositeInstruction +SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instruction, + MoveInstructionPoly& prev_seed, + const CompositeInstruction& instructions, + const tesseract_scene_graph::SceneState& start_state, + const PlannerRequest& request) const { CompositeInstruction seed(instructions); seed.clear(); @@ -162,8 +166,8 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (instruction.isCompositeInstruction()) { - seed.push_back( - processCompositeInstruction(instruction.as(), prev_instruction, prev_seed, request)); + seed.push_back(processCompositeInstruction( + prev_instruction, prev_seed, instruction.as(), start_state, request)); } else if (instruction.isMoveInstruction()) { @@ -171,7 +175,6 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp if (prev_instruction.isNull()) { const std::string manipulator = request.instructions.getManipulatorInfo().manipulator; - const std::string manipulator_ik_solver = request.instructions.getManipulatorInfo().manipulator_ik_solver; tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(manipulator); prev_instruction = base_instruction; @@ -187,7 +190,7 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp // Run IK to find solution closest to start KinematicGroupInstructionInfo info( prev_instruction, *request.env, request.instructions.getManipulatorInfo()); - auto start_seed = getClosestJointSolution(info, request.env_state.getJointValues(manip->getJointNames())); + auto start_seed = getClosestJointSolution(info, start_state.getJointValues(manip->getJointNames())); start_waypoint.as().setSeed( tesseract_common::JointState(manip->getJointNames(), start_seed)); } @@ -216,18 +219,17 @@ 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 = getProfile(name_, + base_instruction.getProfile(name_), + *request.profiles, + std::make_shared()); } 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 = getProfile(name_, + base_instruction.getPathProfile(name_), + *request.profiles, + std::make_shared()); } if (!plan_profile) diff --git a/tesseract_motion_planners/trajopt/CMakeLists.txt b/tesseract_motion_planners/trajopt/CMakeLists.txt index 5ac51385590..cd8c92b8847 100644 --- a/tesseract_motion_planners/trajopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt/CMakeLists.txt @@ -8,6 +8,7 @@ add_library( src/trajopt_waypoint_config.cpp src/trajopt_motion_planner.cpp src/trajopt_utils.cpp + src/profile/trajopt_profile.cpp src/profile/trajopt_default_plan_profile.cpp src/profile/trajopt_default_composite_profile.cpp src/profile/trajopt_default_solver_profile.cpp diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h index 14a17c18cae..1058890e86e 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h @@ -51,12 +51,7 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile using ConstPtr = std::shared_ptr; TrajOptDefaultCompositeProfile() = default; - ~TrajOptDefaultCompositeProfile() override = default; TrajOptDefaultCompositeProfile(const tinyxml2::XMLElement& xml_element); - TrajOptDefaultCompositeProfile(const TrajOptDefaultCompositeProfile&) = default; - TrajOptDefaultCompositeProfile& operator=(const TrajOptDefaultCompositeProfile&) = default; - TrajOptDefaultCompositeProfile(TrajOptDefaultCompositeProfile&&) = default; - TrajOptDefaultCompositeProfile& operator=(TrajOptDefaultCompositeProfile&&) = default; /** @brief The type of contact test to perform: FIRST, CLOSEST, ALL */ tesseract_collision::ContactTestType contact_test_type{ tesseract_collision::ContactTestType::ALL }; @@ -153,7 +148,13 @@ class TrajOptDefaultCompositeProfile : public TrajOptCompositeProfile bool& enabled, Eigen::VectorXd& coeff, std::size_t& length); + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultCompositeProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_COMPOSITE_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h index 83bcdf96710..affec26a22c 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h @@ -45,12 +45,7 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile using ConstPtr = std::shared_ptr; TrajOptDefaultPlanProfile() = default; - ~TrajOptDefaultPlanProfile() override = default; TrajOptDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - TrajOptDefaultPlanProfile(const TrajOptDefaultPlanProfile&) = default; - TrajOptDefaultPlanProfile& operator=(const TrajOptDefaultPlanProfile&) = default; - TrajOptDefaultPlanProfile(TrajOptDefaultPlanProfile&&) = default; - TrajOptDefaultPlanProfile& operator=(TrajOptDefaultPlanProfile&&) = default; CartesianWaypointConfig cartesian_cost_config; CartesianWaypointConfig cartesian_constraint_config; @@ -98,7 +93,13 @@ class TrajOptDefaultPlanProfile : public TrajOptPlanProfile void addConstraintErrorFunctions(trajopt::ProblemConstructionInfo& pci, int index) const; void addAvoidSingularity(trajopt::ProblemConstructionInfo& pci, const std::vector& fixed_steps) const; + + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h index 04f0850ac29..c69e3add39b 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h @@ -38,13 +38,6 @@ class TrajOptDefaultSolverProfile : public TrajOptSolverProfile using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptDefaultSolverProfile() = default; - ~TrajOptDefaultSolverProfile() override = default; - TrajOptDefaultSolverProfile(const TrajOptDefaultSolverProfile&) = default; - TrajOptDefaultSolverProfile& operator=(const TrajOptDefaultSolverProfile&) = default; - TrajOptDefaultSolverProfile(TrajOptDefaultSolverProfile&&) = default; - TrajOptDefaultSolverProfile& operator=(TrajOptDefaultSolverProfile&&) = default; - /** @brief The Convex solver to use */ sco::ModelType convex_solver{ sco::ModelType::OSQP }; @@ -60,6 +53,14 @@ class TrajOptDefaultSolverProfile : public TrajOptSolverProfile void apply(trajopt::ProblemConstructionInfo& pci) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptDefaultSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_DEFAULT_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h index 3bfe558b05a..6a1f07a46f5 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/profile/trajopt_profile.h @@ -31,10 +31,12 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tinyxml2 { @@ -44,18 +46,19 @@ class XMLDocument; namespace tesseract_planning { -class TrajOptPlanProfile +class TrajOptPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptPlanProfile() = default; - virtual ~TrajOptPlanProfile() = default; - TrajOptPlanProfile(const TrajOptPlanProfile&) = default; - TrajOptPlanProfile& operator=(const TrajOptPlanProfile&) = default; - TrajOptPlanProfile(TrajOptPlanProfile&&) = default; - TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = default; + TrajOptPlanProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci, const CartesianWaypointPoly& cartesian_waypoint, @@ -72,20 +75,26 @@ class TrajOptPlanProfile int index) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class TrajOptCompositeProfile +class TrajOptCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptCompositeProfile() = default; - virtual ~TrajOptCompositeProfile() = default; - TrajOptCompositeProfile(const TrajOptCompositeProfile&) = default; - TrajOptCompositeProfile& operator=(const TrajOptCompositeProfile&) = default; - TrajOptCompositeProfile(TrajOptCompositeProfile&&) = default; - TrajOptCompositeProfile& operator=(TrajOptCompositeProfile&&) = default; + TrajOptCompositeProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci, int start_index, @@ -95,26 +104,41 @@ class TrajOptCompositeProfile const std::vector& fixed_indices) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class TrajOptSolverProfile +class TrajOptSolverProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptSolverProfile() = default; - virtual ~TrajOptSolverProfile() = default; - TrajOptSolverProfile(const TrajOptSolverProfile&) = default; - TrajOptSolverProfile& operator=(const TrajOptSolverProfile&) = default; - TrajOptSolverProfile(TrajOptSolverProfile&&) = default; - TrajOptSolverProfile& operator=(TrajOptSolverProfile&&) = default; + TrajOptSolverProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(trajopt::ProblemConstructionInfo& pci) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptCompositeProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_PROFILE_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h index dd239dfc3c0..c26929f7938 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_collision_config.h @@ -30,6 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -70,6 +72,11 @@ struct CollisionCostConfig double coeff = 20; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; /** @@ -97,7 +104,15 @@ struct CollisionConstraintConfig double coeff = 20; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CollisionCostConfig) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CollisionConstraintConfig) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_COLLISION_CONFIG_H diff --git a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h index eb57605eb94..e09acb3ae89 100644 --- a/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h +++ b/tesseract_motion_planners/trajopt/include/tesseract_motion_planners/trajopt/trajopt_waypoint_config.h @@ -29,6 +29,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tinyxml2 @@ -70,6 +72,11 @@ struct CartesianWaypointConfig Eigen::Matrix coeff{ Eigen::VectorXd::Constant(6, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; /** @@ -100,7 +107,15 @@ struct JointWaypointConfig Eigen::VectorXd coeff{ Eigen::VectorXd::Constant(1, 1, 5) }; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::CartesianWaypointConfig) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::JointWaypointConfig) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_CONFIG_TRAJOPT_WAYPOINT_CONFIG_H diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp index a4d5369e6ae..199942a75de 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp @@ -29,6 +29,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,6 +44,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01; @@ -463,4 +468,29 @@ void TrajOptDefaultCompositeProfile::addAvoidSingularity(trajopt::ProblemConstru pci.cost_infos.push_back(ti); } +template +void TrajOptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptCompositeProfile); + ar& BOOST_SERIALIZATION_NVP(contact_test_type); + ar& BOOST_SERIALIZATION_NVP(collision_cost_config); + ar& BOOST_SERIALIZATION_NVP(collision_constraint_config); + ar& BOOST_SERIALIZATION_NVP(smooth_velocities); + ar& BOOST_SERIALIZATION_NVP(velocity_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_accelerations); + ar& BOOST_SERIALIZATION_NVP(acceleration_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_jerks); + ar& BOOST_SERIALIZATION_NVP(jerk_coeff); + ar& BOOST_SERIALIZATION_NVP(avoid_singularity); + ar& BOOST_SERIALIZATION_NVP(avoid_singularity_coeff); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_fraction); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(special_collision_cost); + ar& BOOST_SERIALIZATION_NVP(special_collision_constraint); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultCompositeProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp index e2cc8a29a64..63b96df67f6 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_plan_profile.cpp @@ -27,6 +27,8 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -311,4 +313,19 @@ tinyxml2::XMLElement* TrajOptDefaultPlanProfile::toXML(tinyxml2::XMLDocument& do return xml_planner; } + +template +void TrajOptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptPlanProfile); + ar& BOOST_SERIALIZATION_NVP(cartesian_cost_config); + ar& BOOST_SERIALIZATION_NVP(cartesian_constraint_config); + ar& BOOST_SERIALIZATION_NVP(joint_cost_config); + ar& BOOST_SERIALIZATION_NVP(joint_constraint_config); + // ar& BOOST_SERIALIZATION_NVP(constraint_error_functions); /** @todo FIX */ +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultPlanProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp index 0332baf2de0..b38cbbdccd1 100644 --- a/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_solver_profile.cpp @@ -28,6 +28,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -44,4 +46,19 @@ void TrajOptDefaultSolverProfile::apply(trajopt::ProblemConstructionInfo& pci) c tinyxml2::XMLElement* TrajOptDefaultSolverProfile::toXML(tinyxml2::XMLDocument& /*doc*/) const { return nullptr; } +template +void TrajOptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptSolverProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(convex_solver); + // ar& BOOST_SERIALIZATION_NVP(convex_solver_config); + // ar& BOOST_SERIALIZATION_NVP(opt_info); + // ar& BOOST_SERIALIZATION_NVP(callbacks); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptDefaultSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptDefaultSolverProfile) diff --git a/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp new file mode 100644 index 00000000000..190c23d1e47 --- /dev/null +++ b/tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp @@ -0,0 +1,74 @@ +/** + * @file trajopt_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include + +namespace tesseract_planning +{ +TrajOptPlanProfile::TrajOptPlanProfile() : Profile(TrajOptPlanProfile::getStaticKey()) {} + +std::size_t TrajOptPlanProfile::getStaticKey() { return std::type_index(typeid(TrajOptPlanProfile)).hash_code(); } + +template +void TrajOptPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +TrajOptCompositeProfile::TrajOptCompositeProfile() : Profile(TrajOptCompositeProfile::getStaticKey()) {} + +std::size_t TrajOptCompositeProfile::getStaticKey() +{ + return std::type_index(typeid(TrajOptCompositeProfile)).hash_code(); +} + +template +void TrajOptCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +TrajOptSolverProfile::TrajOptSolverProfile() : Profile(TrajOptSolverProfile::getStaticKey()) {} + +std::size_t TrajOptSolverProfile::getStaticKey() { return std::type_index(typeid(TrajOptSolverProfile)).hash_code(); } + +template +void TrajOptSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptCompositeProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptSolverProfile) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp index c725c2b08c0..8a12453479a 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_collision_config.cpp @@ -29,6 +29,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -140,6 +141,17 @@ tinyxml2::XMLElement* CollisionCostConfig::toXML(tinyxml2::XMLDocument& doc) con return xml_coll_cost_config; } +template +void CollisionCostConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_weighted_sum); + ar& BOOST_SERIALIZATION_NVP(type); + ar& BOOST_SERIALIZATION_NVP(safety_margin); + ar& BOOST_SERIALIZATION_NVP(safety_margin_buffer); + ar& BOOST_SERIALIZATION_NVP(coeff); +} + CollisionConstraintConfig::CollisionConstraintConfig(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); @@ -243,4 +255,21 @@ tinyxml2::XMLElement* CollisionConstraintConfig::toXML(tinyxml2::XMLDocument& do return xml_coll_cnt_config; } + +template +void CollisionConstraintConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_weighted_sum); + ar& BOOST_SERIALIZATION_NVP(type); + ar& BOOST_SERIALIZATION_NVP(safety_margin); + ar& BOOST_SERIALIZATION_NVP(safety_margin_buffer); + ar& BOOST_SERIALIZATION_NVP(coeff); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CollisionCostConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CollisionCostConfig) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CollisionConstraintConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CollisionConstraintConfig) diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 2cd3e796e1a..4959dac7b79 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -225,12 +225,11 @@ 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); + TrajOptSolverProfile::ConstPtr solver_profile = + getProfile(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared()); if (!solver_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -265,10 +264,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()); + name_, move_instruction.getProfile(name_), *request.profiles, std::make_shared()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -285,7 +282,7 @@ TrajOptMotionPlanner::createProblem(const PlannerRequest& request) const } else { - seed_states.push_back(request.env_state.getJointValues(joint_names)); + seed_states.push_back(request.env->getCurrentJointValues(joint_names)); } /** @todo If fixed cartesian and not term_type cost add as fixed */ @@ -348,11 +345,12 @@ 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()); + TrajOptCompositeProfile::ConstPtr cur_composite_profile = + getProfile(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared()); + if (!cur_composite_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); diff --git a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp index e10e0ae8467..f2db1b9fb65 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_waypoint_config.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_planning { @@ -154,6 +155,16 @@ tinyxml2::XMLElement* CartesianWaypointConfig::toXML(tinyxml2::XMLDocument& doc) return xml_cartesian_waypoint_config; } +template +void CartesianWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_tolerance_override); + ar& BOOST_SERIALIZATION_NVP(lower_tolerance); + ar& BOOST_SERIALIZATION_NVP(upper_tolerance); + ar& BOOST_SERIALIZATION_NVP(coeff); +} + JointWaypointConfig::JointWaypointConfig(const tinyxml2::XMLElement& xml_element) { const tinyxml2::XMLElement* enabled_element = xml_element.FirstChildElement("Enabled"); @@ -271,4 +282,20 @@ tinyxml2::XMLElement* JointWaypointConfig::toXML(tinyxml2::XMLDocument& doc) con return xml_cartesian_waypoint_config; } + +template +void JointWaypointConfig::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_NVP(enabled); + ar& BOOST_SERIALIZATION_NVP(use_tolerance_override); + ar& BOOST_SERIALIZATION_NVP(lower_tolerance); + ar& BOOST_SERIALIZATION_NVP(upper_tolerance); + ar& BOOST_SERIALIZATION_NVP(coeff); +} } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::CartesianWaypointConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::CartesianWaypointConfig) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::JointWaypointConfig) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::JointWaypointConfig) diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 446fa1ec105..75bd1867e35 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -46,6 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include #include #include @@ -119,7 +120,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -142,17 +142,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -161,7 +162,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; // Loop over all combinations of these 4. 0001, 0010, 0011, ... , 1111 @@ -194,7 +194,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -217,17 +216,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -236,7 +236,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -281,7 +280,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT { auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start JointWaypointPoly wp1{ JointWaypoint(joint_names, Eigen::VectorXd::Zero(7)) }; @@ -304,17 +302,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -323,7 +322,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -366,12 +364,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // This test tests freespace motion b/n 1 cartesian waypoint and 1 joint waypoint TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Specify a JointWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.2) * @@ -395,17 +389,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -414,7 +409,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -457,12 +451,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // This test tests freespace motion b/n 2 cartesian waypoints TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - auto joint_group = env_->getJointGroup(manip.manipulator); - auto cur_state = env_->getState(); - // Specify a CartesianWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.2) * Eigen::Quaterniond(0, 0, 1.0, 0)) }; @@ -486,17 +474,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -505,7 +494,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; { @@ -548,12 +536,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // are / added correctly TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - - auto joint_group = env_->getJointGroup(manip.manipulator); - auto cur_state = env_->getState(); - // Specify a JointWaypoint as the start CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity() * Eigen::Translation3d(-.20, .4, 0.8) * Eigen::Quaterniond(0, 0, 1.0, 0)) }; @@ -577,17 +559,18 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL program.appendMoveInstruction(plan_f1); // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // 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", 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); @@ -596,7 +579,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci; @@ -642,12 +624,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // This test checks that the terms are being added correctly for joint cnts TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); std::vector joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Create a program CompositeInstruction program("TEST_PROFILE"); @@ -664,8 +642,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT } // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -674,11 +651,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT plan_profile->joint_cost_config.enabled = false; plan_profile->joint_constraint_config.enabled = true; 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", 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,7 +666,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci = test_planner.createProblem(request); @@ -706,12 +684,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // This test checks that the terms are being added correctly for joint costs TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT { - // Create the planner and the responses that will store the results - PlannerResponse planning_response; - auto joint_group = env_->getJointGroup(manip.manipulator); const std::vector& joint_names = joint_group->getJointNames(); - auto cur_state = env_->getState(); // Create a program CompositeInstruction program("TEST_PROFILE"); @@ -728,8 +702,7 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT } // Create a seed - CompositeInstruction interpolated_program = - generateInterpolatedProgram(program, cur_state, env_, 3.14, 1.0, 3.14, 10); + CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles auto plan_profile = std::make_shared(); @@ -739,11 +712,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT plan_profile->joint_constraint_config.enabled = false; 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", 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); @@ -752,7 +727,6 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT PlannerRequest request; request.instructions = interpolated_program; request.env = env_; - request.env_state = cur_state; request.profiles = profiles; std::shared_ptr pci = test_planner.createProblem(request); diff --git a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt index 711c515ac29..7f03805c0b9 100644 --- a/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt +++ b/tesseract_motion_planners/trajopt_ifopt/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( src/trajopt_ifopt_motion_planner.cpp src/trajopt_ifopt_problem.cpp src/trajopt_ifopt_utils.cpp + src/profile/trajopt_ifopt_profile.cpp src/profile/trajopt_ifopt_default_plan_profile.cpp src/profile/trajopt_ifopt_default_composite_profile.cpp src/profile/trajopt_ifopt_default_solver_profile.cpp) diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h index c4ca46dc05c..2d7fb81c96b 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_composite_profile.h @@ -90,7 +90,14 @@ class TrajOptIfoptDefaultCompositeProfile : public TrajOptIfoptCompositeProfile const std::vector& fixed_indices) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptDefaultCompositeProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_COMPOSITE_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h index 9676c365dd7..01264d5c1a3 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_plan_profile.h @@ -65,7 +65,14 @@ class TrajOptIfoptDefaultPlanProfile : public TrajOptIfoptPlanProfile int index) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptDefaultPlanProfile) + #endif // TESSERACT_MOTION_PLANNERS_TrajOptIfopt_IFOPT_DEFAULT_PLAN_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h index 80be4dbb703..04ee00a461d 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_default_solver_profile.h @@ -53,8 +53,8 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile ~TrajOptIfoptDefaultSolverProfile() override; TrajOptIfoptDefaultSolverProfile(const TrajOptIfoptDefaultSolverProfile&) = delete; TrajOptIfoptDefaultSolverProfile& operator=(const TrajOptIfoptDefaultSolverProfile&) = delete; - TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = default; - TrajOptIfoptDefaultSolverProfile& operator=(TrajOptIfoptDefaultSolverProfile&&) = default; + TrajOptIfoptDefaultSolverProfile(TrajOptIfoptDefaultSolverProfile&&) = delete; + TrajOptIfoptDefaultSolverProfile&& operator=(TrajOptIfoptDefaultSolverProfile&&) = delete; /** @brief The OSQP convex solver settings to use * @todo Replace by convex_solver_config (cf. sco::ModelConfig) once solver selection is possible */ @@ -69,6 +69,14 @@ class TrajOptIfoptDefaultSolverProfile : public TrajOptIfoptSolverProfile void apply(TrajOptIfoptProblem& problem) const override; tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptDefaultSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_DEFAULT_SOLVER_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h index 35b62e19826..2aadefeb10d 100644 --- a/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h +++ b/tesseract_motion_planners/trajopt_ifopt/include/tesseract_motion_planners/trajopt_ifopt/profile/trajopt_ifopt_profile.h @@ -33,8 +33,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP -#include #include +#include +#include namespace tinyxml2 { @@ -46,18 +47,19 @@ namespace tesseract_planning { struct TrajOptIfoptProblem; -class TrajOptIfoptPlanProfile +class TrajOptIfoptPlanProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptPlanProfile() = default; - virtual ~TrajOptIfoptPlanProfile() = default; - TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = default; - TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = default; - TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = default; - TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = default; + TrajOptIfoptPlanProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem, const CartesianWaypointPoly& cartesian_waypoint, @@ -74,20 +76,26 @@ class TrajOptIfoptPlanProfile int index) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class TrajOptIfoptCompositeProfile +class TrajOptIfoptCompositeProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptCompositeProfile() = default; - virtual ~TrajOptIfoptCompositeProfile() = default; - TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = default; - TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = default; - TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = default; - TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = default; + TrajOptIfoptCompositeProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem, int start_index, @@ -97,26 +105,41 @@ class TrajOptIfoptCompositeProfile const std::vector& fixed_indices) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; -class TrajOptIfoptSolverProfile +class TrajOptIfoptSolverProfile : public Profile { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TrajOptIfoptSolverProfile() = default; - virtual ~TrajOptIfoptSolverProfile() = default; - TrajOptIfoptSolverProfile(const TrajOptIfoptSolverProfile&) = default; - TrajOptIfoptSolverProfile& operator=(const TrajOptIfoptSolverProfile&) = default; - TrajOptIfoptSolverProfile(TrajOptIfoptSolverProfile&&) = default; - TrajOptIfoptSolverProfile& operator=(TrajOptIfoptSolverProfile&&) = default; + TrajOptIfoptSolverProfile(); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); virtual void apply(TrajOptIfoptProblem& problem) const = 0; virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptPlanProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptCompositeProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TrajOptIfoptSolverProfile) + #endif // TESSERACT_MOTION_PLANNERS_TRAJOPT_IFOPT_PROFILE_H diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp index 290dc437d62..0a9ae17a8f1 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_composite_profile.cpp @@ -29,6 +29,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,6 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include namespace tesseract_planning { @@ -97,4 +103,26 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultCompositeProfile::toXML(tinyxml2::XMLDo throw std::runtime_error("TrajOptIfoptDefaultCompositeProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptCompositeProfile); + ar& BOOST_SERIALIZATION_NVP(collision_cost_config); + ar& BOOST_SERIALIZATION_NVP(collision_constraint_config); + ar& BOOST_SERIALIZATION_NVP(smooth_velocities); + ar& BOOST_SERIALIZATION_NVP(velocity_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_accelerations); + ar& BOOST_SERIALIZATION_NVP(acceleration_coeff); + ar& BOOST_SERIALIZATION_NVP(smooth_jerks); + ar& BOOST_SERIALIZATION_NVP(jerk_coeff); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_fraction); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); + ar& BOOST_SERIALIZATION_NVP(special_collision_cost); + ar& BOOST_SERIALIZATION_NVP(special_collision_constraint); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultCompositeProfile) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp index 4283be0dd5e..4627f90d3c8 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_plan_profile.cpp @@ -29,6 +29,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -41,6 +43,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + namespace tesseract_planning { void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, @@ -132,4 +136,17 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultPlanProfile::toXML(tinyxml2::XMLDocumen throw std::runtime_error("TrajOptIfoptDefaultPlanProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptPlanProfile); + ar& BOOST_SERIALIZATION_NVP(cartesian_coeff); + ar& BOOST_SERIALIZATION_NVP(joint_coeff); + ar& BOOST_SERIALIZATION_NVP(term_type); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultPlanProfile) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp index c7e149fa93c..e6471c6dddc 100644 --- a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_default_solver_profile.cpp @@ -28,6 +28,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -62,4 +65,18 @@ tinyxml2::XMLElement* TrajOptIfoptDefaultSolverProfile::toXML(tinyxml2::XMLDocum throw std::runtime_error("TrajOptIfoptDefaultSolverProfile::toXML is not implemented!"); } +template +void TrajOptIfoptDefaultSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(TrajOptIfoptSolverProfile); + /** @todo FIX */ + // ar& BOOST_SERIALIZATION_NVP(convex_solver_settings); + // ar& BOOST_SERIALIZATION_NVP(opt_info); + // ar& BOOST_SERIALIZATION_NVP(callbacks); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptDefaultSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptDefaultSolverProfile) diff --git a/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp new file mode 100644 index 00000000000..5f5f7dac869 --- /dev/null +++ b/tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp @@ -0,0 +1,80 @@ +/** + * @file trajopt_ifopt_profile.cpp + * @brief + * + * @author Levi Armstrong + * @date June 18, 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include + +namespace tesseract_planning +{ +TrajOptIfoptPlanProfile::TrajOptIfoptPlanProfile() : Profile(TrajOptIfoptPlanProfile::getStaticKey()) {} + +std::size_t TrajOptIfoptPlanProfile::getStaticKey() +{ + return std::type_index(typeid(TrajOptIfoptPlanProfile)).hash_code(); +} + +template +void TrajOptIfoptPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +TrajOptIfoptCompositeProfile::TrajOptIfoptCompositeProfile() : Profile(TrajOptIfoptCompositeProfile::getStaticKey()) {} + +std::size_t TrajOptIfoptCompositeProfile::getStaticKey() +{ + return std::type_index(typeid(TrajOptIfoptCompositeProfile)).hash_code(); +} + +template +void TrajOptIfoptCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +TrajOptIfoptSolverProfile::TrajOptIfoptSolverProfile() : Profile(TrajOptIfoptSolverProfile::getStaticKey()) {} + +std::size_t TrajOptIfoptSolverProfile::getStaticKey() +{ + return std::type_index(typeid(TrajOptIfoptSolverProfile)).hash_code(); +} + +template +void TrajOptIfoptSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptPlanProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptPlanProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptCompositeProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptCompositeProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TrajOptIfoptSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TrajOptIfoptSolverProfile) 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 52dc9122803..2d2c3d3b235 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 @@ -178,7 +178,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co // Create the problem auto problem = std::make_shared(); problem->environment = request.env; - problem->env_state = request.env_state; + problem->env_state = request.env->getState(); problem->nlp = std::make_shared(); // Assume all the plan instructions have the same manipulator as the composite @@ -226,12 +226,11 @@ 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, request.plan_profile_remapping); - TrajOptIfoptSolverProfile::ConstPtr solver_profile = getProfile( - name_, profile, *request.profiles, std::make_shared()); - solver_profile = applyProfileOverrides(name_, profile, solver_profile, profile_overrides); + TrajOptIfoptSolverProfile::ConstPtr solver_profile = + getProfile(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared()); if (!solver_profile) throw std::runtime_error("TrajOptIfoptMotionPlanner: Invalid profile"); @@ -266,10 +265,11 @@ 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()); + TrajOptIfoptPlanProfile::ConstPtr cur_plan_profile = + getProfile(name_, + move_instruction.getProfile(name_), + *request.profiles, + std::make_shared()); if (!cur_plan_profile) throw std::runtime_error("TrajOptMotionPlanner: Invalid profile"); @@ -286,7 +286,7 @@ std::shared_ptr TrajOptIfoptMotionPlanner::createProblem(co } else { - seed_state = request.env_state.getJointValues(joint_names); + seed_state = request.env->getCurrentJointValues(joint_names); } // Add variable set to problem @@ -354,11 +354,12 @@ 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()); + TrajOptIfoptCompositeProfile::ConstPtr cur_composite_profile = + getProfile(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared()); + if (!cur_composite_profile) throw std::runtime_error("DefaultTrajoptIfoptProblemGenerator: Invalid profile"); diff --git a/tesseract_task_composer/config/task_composer_plugins.yaml b/tesseract_task_composer/config/task_composer_plugins.yaml index 59869e1a16b..40393ff49b8 100644 --- a/tesseract_task_composer/config/task_composer_plugins.yaml +++ b/tesseract_task_composer/config/task_composer_plugins.yaml @@ -39,7 +39,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -51,8 +50,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -65,7 +62,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -75,8 +71,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -151,7 +145,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -163,8 +156,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -177,7 +168,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -187,8 +177,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -263,7 +251,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -275,8 +262,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -289,8 +274,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -363,7 +346,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -375,8 +357,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -389,8 +369,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -463,7 +441,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data OMPLMotionPlannerTask: @@ -475,8 +452,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -489,7 +464,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -499,8 +473,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -575,7 +547,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data TrajOptMotionPlannerTask: @@ -587,8 +558,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -601,7 +570,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -611,8 +579,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -687,7 +653,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data TrajOptIfoptMotionPlannerTask: @@ -699,8 +664,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -713,7 +676,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -723,8 +685,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -799,7 +759,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -811,8 +770,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -825,8 +782,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -839,7 +794,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -849,8 +803,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -927,7 +879,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data OMPLMotionPlannerTask: @@ -939,8 +890,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -953,8 +902,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -967,7 +914,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -977,8 +923,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -1055,7 +999,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data OMPLMotionPlannerTask: @@ -1067,8 +1010,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1081,8 +1022,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -1095,7 +1034,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -1105,8 +1043,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -1184,8 +1120,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1289,8 +1223,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1394,8 +1326,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1492,8 +1422,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1590,8 +1518,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1604,8 +1530,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1711,8 +1635,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1725,8 +1647,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1832,8 +1752,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1846,8 +1764,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1946,8 +1862,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1960,8 +1874,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true diff --git a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml index a2f00db1ee1..02223496823 100644 --- a/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml +++ b/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml @@ -39,7 +39,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -51,8 +50,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -65,7 +62,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -75,8 +71,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -106,6 +100,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesFTask config: @@ -113,6 +116,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesDTask: @@ -140,7 +145,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -152,8 +156,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -166,7 +168,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -176,8 +177,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -207,6 +206,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesDTask config: @@ -214,6 +222,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesFNPCTask: @@ -241,7 +251,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -253,8 +262,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -267,8 +274,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -296,6 +301,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesFNPCTask config: @@ -303,6 +317,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] DescartesDNPCTask: @@ -330,7 +346,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -342,8 +357,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -356,8 +369,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -385,6 +396,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: DescartesDNPCTask config: @@ -392,6 +412,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] OMPLTask: @@ -419,7 +441,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data OMPLMotionPlannerTask: @@ -431,8 +452,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -445,7 +464,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -455,8 +473,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -486,6 +502,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: OMPLTask config: @@ -493,6 +518,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] TrajOptTask: @@ -520,7 +547,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data TrajOptMotionPlannerTask: @@ -532,8 +558,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -546,7 +570,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -556,8 +579,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -587,6 +608,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: TrajOptTask config: @@ -594,6 +624,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] CartesianTask: @@ -621,7 +653,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data DescartesMotionPlannerTask: @@ -633,8 +664,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -647,8 +676,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -661,7 +688,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -671,8 +697,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -704,6 +728,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: CartesianTask config: @@ -711,6 +744,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] FreespaceTask: @@ -738,7 +773,6 @@ task_composer_plugins: program: input_data environment: environment profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data OMPLMotionPlannerTask: @@ -750,8 +784,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -764,8 +796,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false @@ -778,7 +808,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping IterativeSplineParameterizationTask: class: IterativeSplineParameterizationTaskFactory config: @@ -788,8 +817,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data edges: @@ -821,6 +848,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: FreespaceTask config: @@ -828,6 +864,128 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask + destinations: [MotionPlanningTask] + terminals: [MotionPlanningTask] + FreespaceIfoptTask: + class: PipelineTaskFactory + config: + conditional: true + inputs: + program: input_data + outputs: + program: output_data + nodes: + DoneTask: + class: DoneTaskFactory + config: + conditional: false + ErrorTask: + class: ErrorTaskFactory + config: + conditional: false + MinLengthTask: + class: MinLengthTaskFactory + config: + conditional: true + inputs: + program: input_data + environment: environment + profiles: profiles + outputs: + program: output_data + OMPLMotionPlannerTask: + class: OMPLMotionPlannerTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + outputs: + program: output_data + format_result_as_input: true + TrajOptIfoptMotionPlannerTask: + class: TrajOptIfoptMotionPlannerTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + outputs: + program: output_data + format_result_as_input: false + DiscreteContactCheckTask: + class: DiscreteContactCheckTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + IterativeSplineParameterizationTask: + class: IterativeSplineParameterizationTaskFactory + config: + conditional: true + inputs: + program: output_data + environment: environment + profiles: profiles + manip_info: manip_info + outputs: + program: output_data + edges: + - source: MinLengthTask + destinations: [ErrorTask, OMPLMotionPlannerTask] + - source: OMPLMotionPlannerTask + destinations: [ErrorTask, TrajOptIfoptMotionPlannerTask] + - source: TrajOptIfoptMotionPlannerTask + destinations: [ErrorTask, DiscreteContactCheckTask] + - source: DiscreteContactCheckTask + destinations: [ErrorTask, IterativeSplineParameterizationTask] + - source: IterativeSplineParameterizationTask + destinations: [ErrorTask, DoneTask] + terminals: [ErrorTask, DoneTask] + FreespaceIfoptPipeline: + class: PipelineTaskFactory + config: + conditional: false + inputs: + planning_input: planning_input + outputs: + program: output_data + nodes: + ProcessInputTask: + class: ProcessPlanningInputTaskFactory + config: + conditional: false + inputs: + planning_input: planning_input + outputs: + program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data + MotionPlanningTask: + task: FreespaceIfoptTask + config: + conditional: false + abort_terminal: 0 + edges: + - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtTask: @@ -856,8 +1014,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -915,6 +1071,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtTask config: @@ -922,6 +1087,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtTask: @@ -950,8 +1117,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1009,6 +1174,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtTask config: @@ -1016,6 +1190,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyTask: @@ -1044,8 +1220,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1096,6 +1270,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtOnlyTask config: @@ -1103,6 +1286,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyTask: @@ -1131,8 +1316,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1183,6 +1366,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtOnlyTask config: @@ -1190,6 +1382,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtGlobalTask: @@ -1218,8 +1412,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1232,8 +1424,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1293,6 +1483,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtGlobalTask config: @@ -1300,6 +1499,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtGlobalTask: @@ -1328,8 +1529,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1342,8 +1541,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1403,6 +1600,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtGlobalTask config: @@ -1410,6 +1616,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterFtOnlyGlobalTask: @@ -1438,8 +1646,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1452,8 +1658,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1506,6 +1710,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterFtOnlyGlobalTask config: @@ -1513,6 +1726,8 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] RasterCtOnlyGlobalTask: @@ -1541,8 +1756,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1555,8 +1768,6 @@ task_composer_plugins: environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: true @@ -1609,6 +1820,15 @@ task_composer_plugins: planning_input: planning_input outputs: program: input_data + FormatInputTask: + class: FormatPlanningInputTaskFactory + config: + conditional: false + inputs: + program: input_data + environment: environment + outputs: + program: input_data MotionPlanningTask: task: RasterCtOnlyGlobalTask config: @@ -1616,5 +1836,7 @@ task_composer_plugins: abort_terminal: 0 edges: - source: ProcessInputTask + destinations: [FormatInputTask] + - source: FormatInputTask destinations: [MotionPlanningTask] terminals: [MotionPlanningTask] diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h index 78d18cab277..2dbc602092b 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node.h @@ -40,7 +40,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include +#include namespace YAML { @@ -52,15 +54,6 @@ namespace tesseract_planning class TaskComposerDataStorage; class TaskComposerContext; class TaskComposerExecutor; -class TaskComposerNodeInfo; - -enum class TaskComposerNodeType -{ - NODE, - TASK, - PIPELINE, - GRAPH -}; /** @brief Represents a node the pipeline to be executed */ class TaskComposerNode @@ -71,6 +64,9 @@ class TaskComposerNode using UPtr = std::unique_ptr; using ConstUPtr = std::unique_ptr; + // @brief The results map + using ResultsMap = std::map>; + /** @brief Most task will not require a executor so making it optional */ using OptionalTaskComposerExecutor = std::optional>; @@ -148,12 +144,10 @@ class TaskComposerNode TaskComposerNodePorts getPorts() const; /** @brief Generate the Dotgraph as a string */ - std::string - getDotgraph(const std::map>& results_map = {}) const; + std::string getDotgraph(const ResultsMap& results_map = ResultsMap()) const; /** @brief Generate the Dotgraph and save to file */ - bool saveDotgraph(const std::string& filepath, - const std::map>& results_map = {}) const; + bool saveDotgraph(const std::string& filepath, const ResultsMap& results_map = ResultsMap()) const; // NOLINT /** @brief Rename input keys */ virtual void renameInputKeys(const std::map& input_keys); @@ -168,10 +162,9 @@ class TaskComposerNode * @brief dump the task to dot * @brief Return additional subgraphs which should get appended if needed */ - virtual std::string - dump(std::ostream& os, - const TaskComposerNode* parent = nullptr, - const std::map>& results_map = {}) const; + virtual std::string dump(std::ostream& os, + const TaskComposerNode* parent = nullptr, + const ResultsMap& results_map = ResultsMap()) const; bool operator==(const TaskComposerNode& rhs) const; bool operator!=(const TaskComposerNode& rhs) const; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h index f8cdaec107d..3dacf256a94 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_info.h @@ -40,12 +40,14 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include -#include +#include #include #include namespace tesseract_planning { +class TaskComposerNode; + /** Stores information about a node */ class TaskComposerNodeInfo { @@ -57,7 +59,7 @@ class TaskComposerNodeInfo TaskComposerNodeInfo() = default; // Required for serialization TaskComposerNodeInfo(const TaskComposerNode& node); - ~TaskComposerNodeInfo() = default; + ~TaskComposerNodeInfo(); TaskComposerNodeInfo(const TaskComposerNodeInfo&) = default; TaskComposerNodeInfo& operator=(const TaskComposerNodeInfo&) = default; TaskComposerNodeInfo(TaskComposerNodeInfo&&) = default; diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_types.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_types.h new file mode 100644 index 00000000000..41489a9aa10 --- /dev/null +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_types.h @@ -0,0 +1,40 @@ +/** + * @file task_composer_node_types.h + * @brief A node types enum + * + * @author Levi Armstrong + * @date July 29. 2022 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2022, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_TASK_COMPOSER_TASK_COMPOSER_NODE_TYPES_H +#define TESSERACT_TASK_COMPOSER_TASK_COMPOSER_NODE_TYPES_H + +namespace tesseract_planning +{ +enum class TaskComposerNodeType +{ + NODE, + TASK, + PIPELINE, + GRAPH +}; +} + +#endif // TESSERACT_TASK_COMPOSER_TASK_COMPOSER_NODE_TYPES_H diff --git a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h index fe4a8a8131c..69ac8003356 100644 --- a/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h +++ b/tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_plugin_factory.h @@ -100,8 +100,8 @@ class TaskComposerPluginFactory ~TaskComposerPluginFactory(); TaskComposerPluginFactory(const TaskComposerPluginFactory&) = delete; TaskComposerPluginFactory& operator=(const TaskComposerPluginFactory&) = delete; - TaskComposerPluginFactory(TaskComposerPluginFactory&&) = default; - TaskComposerPluginFactory& operator=(TaskComposerPluginFactory&&) = default; + TaskComposerPluginFactory(TaskComposerPluginFactory&&) noexcept = default; + TaskComposerPluginFactory& operator=(TaskComposerPluginFactory&&) noexcept = default; /** * @brief Load plugins from a configuration object diff --git a/tesseract_task_composer/core/src/task_composer_node.cpp b/tesseract_task_composer/core/src/task_composer_node.cpp index 649c359dc90..83727b25899 100644 --- a/tesseract_task_composer/core/src/task_composer_node.cpp +++ b/tesseract_task_composer/core/src/task_composer_node.cpp @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include #include #include @@ -384,8 +385,7 @@ const TaskComposerKeys& TaskComposerNode::getOutputKeys() const { return output_ TaskComposerNodePorts TaskComposerNode::getPorts() const { return ports_; } -std::string TaskComposerNode::getDotgraph( - const std::map>& results_map) const +std::string TaskComposerNode::getDotgraph(const ResultsMap& results_map) const { try { @@ -402,9 +402,7 @@ std::string TaskComposerNode::getDotgraph( return {}; } -bool TaskComposerNode::saveDotgraph( - const std::string& filepath, - const std::map>& results_map) const +bool TaskComposerNode::saveDotgraph(const std::string& filepath, const ResultsMap& results_map) const { try { @@ -435,10 +433,9 @@ void TaskComposerNode::renameOutputKeys(const std::map void TaskComposerNode::setConditional(bool enable) { conditional_ = enable; } -std::string -TaskComposerNode::dump(std::ostream& os, - const TaskComposerNode* /*parent*/, - const std::map>& results_map) const +std::string TaskComposerNode::dump(std::ostream& os, + const TaskComposerNode* /*parent*/, + const ResultsMap& results_map) const { const std::string tmp = toString(uuid_, "node_"); @@ -543,18 +540,8 @@ void TaskComposerNode::serialize(Archive& ar, const unsigned int /*version*/) std::string TaskComposerNode::toString(const boost::uuids::uuid& u, const std::string& prefix) { - std::string result; - result.reserve(36); - - std::size_t i = 0; - for (const auto* it_data = u.begin(); it_data != u.end(); ++it_data, ++i) - { - const size_t hi = ((*it_data) >> 4) & 0x0F; - result += boost::uuids::detail::to_char(hi); - - const size_t lo = (*it_data) & 0x0F; - result += boost::uuids::detail::to_char(lo); - } + auto result = boost::uuids::to_string(u); + boost::replace_all(result, "-", ""); return (prefix + result); } diff --git a/tesseract_task_composer/core/src/task_composer_node_info.cpp b/tesseract_task_composer/core/src/task_composer_node_info.cpp index 2a859c9f09e..b970e6cd0dd 100644 --- a/tesseract_task_composer/core/src/task_composer_node_info.cpp +++ b/tesseract_task_composer/core/src/task_composer_node_info.cpp @@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include namespace tesseract_planning @@ -66,6 +67,8 @@ TaskComposerNodeInfo::TaskComposerNodeInfo(const TaskComposerNode& node) } } +TaskComposerNodeInfo::~TaskComposerNodeInfo() = default; + bool TaskComposerNodeInfo::operator==(const TaskComposerNodeInfo& rhs) const { static auto max_diff = static_cast(std::numeric_limits::epsilon()); diff --git a/tesseract_task_composer/examples/task_composer_raster_example.cpp b/tesseract_task_composer/examples/task_composer_raster_example.cpp index bb3a124f152..1302fc9c93b 100644 --- a/tesseract_task_composer/examples/task_composer_raster_example.cpp +++ b/tesseract_task_composer/examples/task_composer_raster_example.cpp @@ -61,6 +61,7 @@ int main() // Define profiles auto profiles = std::make_shared(); + /** @todo Add default profiles */ // 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 fccab3a9da2..764eb0148ad 100644 --- a/tesseract_task_composer/examples/task_composer_trajopt_example.cpp +++ b/tesseract_task_composer/examples/task_composer_trajopt_example.cpp @@ -62,6 +62,7 @@ int main() // Define profiles auto profiles = std::make_shared(); + /** @todo Add default profiles */ // Define the program CompositeInstruction program = test_suite::freespaceExampleProgramIIWA(); diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 3fc1ce07ad7..d42dc32dd0d 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -30,7 +30,15 @@ set(LIB_SOURCE_FILES src/nodes/upsample_trajectory_task.cpp src/nodes/raster_motion_task.cpp src/nodes/raster_only_motion_task.cpp - src/profiles/contact_check_profile.cpp) + src/profiles/contact_check_profile.cpp + src/profiles/fix_state_bounds_profile.cpp + src/profiles/fix_state_collision_profile.cpp + src/profiles/iterative_spline_parameterization_profile.cpp + src/profiles/min_length_profile.cpp + src/profiles/profile_switch_profile.cpp + src/profiles/ruckig_trajectory_smoothing_profile.cpp + src/profiles/time_optimal_parameterization_profile.cpp + src/profiles/upsample_trajectory_profile.cpp) set(LIB_SOURCE_LINK_LIBRARIES ${PROJECT_NAME} diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h index 464e98ecc7e..4d36ecd7126 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/continuous_contact_check_task.h @@ -52,7 +52,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT ContinuousContactCheckTask : // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; static const std::string OUTPUT_CONTACT_RESULTS_PORT; using Ptr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h index 44e67878a0c..d54ab165a46 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/discrete_contact_check_task.h @@ -52,7 +52,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT DiscreteContactCheckTask : p // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; static const std::string OUTPUT_CONTACT_RESULTS_PORT; using Ptr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h index 8c80cc0dcf7..5653d197ef8 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_bounds_task.h @@ -53,7 +53,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT FixStateBoundsTask : public // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h index 7988cbeb725..62b295f8d1b 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/fix_state_collision_task.h @@ -61,7 +61,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT FixStateCollisionTask : publ // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; static const std::string OUTPUT_CONTACT_RESULTS_PORT; using Ptr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h index 3947c5b8ade..34ecc3ece8c 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/iterative_spline_parameterization_task.h @@ -50,8 +50,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT IterativeSplineParameterizat // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - static const std::string INPUT_MOVE_PROFILE_REMAPPING_PORT; using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h index 2afc4c8499e..d26b96e6861 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/min_length_task.h @@ -47,9 +47,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT MinLengthTask : public TaskC static const std::string INPUT_ENVIRONMENT_PORT; static const std::string INPUT_PROFILES_PORT; - // Optional - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using UPtr = std::unique_ptr; 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 ab0d2b9e5be..296f0fa6494 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 @@ -58,8 +58,6 @@ class MotionPlannerTask : public TaskComposerTask // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - static const std::string INPUT_MOVE_PROFILE_REMAPPING_PORT; MotionPlannerTask() : TaskComposerTask("MotionPlannerTask", MotionPlannerTask::ports(), true) {} explicit MotionPlannerTask(std::string name, // NOLINT(performance-unnecessary-value-param) @@ -130,8 +128,6 @@ class MotionPlannerTask : public TaskComposerTask ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_MOVE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; return ports; @@ -170,9 +166,6 @@ class MotionPlannerTask : public TaskComposerTask auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).template as>(); - auto composite_profile_remapping_poly = - getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); - auto move_profile_remapping_poly = getData(*context.data_storage, INPUT_MOVE_PROFILE_REMAPPING_PORT, false); tesseract_common::ManipulatorInfo input_manip_info; auto manip_info_poly = getData(*context.data_storage, INPUT_MANIP_INFO_PORT, false); @@ -188,14 +181,9 @@ class MotionPlannerTask : public TaskComposerTask // Fill out request // -------------------- PlannerRequest request; - request.env_state = env->getState(); request.env = env; request.instructions = instructions; request.profiles = profiles; - if (!move_profile_remapping_poly.isNull()) - request.plan_profile_remapping = move_profile_remapping_poly.template as(); - if (!composite_profile_remapping_poly.isNull()) - request.composite_profile_remapping = composite_profile_remapping_poly.template as(); request.format_result_as_input = format_result_as_input_; // -------------------- @@ -249,13 +237,6 @@ const std::string MotionPlannerTask::INPUT_PROFILES_PORT = "p template const std::string MotionPlannerTask::INPUT_MANIP_INFO_PORT = "manip_info"; -template -const std::string MotionPlannerTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_" - "remapping"; - -template -const std::string MotionPlannerTask::INPUT_MOVE_PROFILE_REMAPPING_PORT = "move_profile_remapping"; - } // namespace tesseract_planning #endif // TESSERACT_TASK_COMPOSER_MOTION_PLANNER_TASK_HPP diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h index d0873270f7e..a4a693718f6 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/profile_switch_task.h @@ -47,9 +47,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT ProfileSwitchTask : public T static const std::string INPUT_PROGRAM_PORT; static const std::string INPUT_PROFILES_PORT; - // Optional - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using UPtr = std::unique_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h index 8ed42834fb9..e55906e8eeb 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/ruckig_trajectory_smoothing_task.h @@ -46,8 +46,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT RuckigTrajectorySmoothingTas // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - static const std::string INPUT_MOVE_PROFILE_REMAPPING_PORT; using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h index 7d29211e11f..ee0669f52e2 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/time_optimal_parameterization_task.h @@ -50,8 +50,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT TimeOptimalParameterizationT // Optional static const std::string INPUT_MANIP_INFO_PORT; - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - static const std::string INPUT_MOVE_PROFILE_REMAPPING_PORT; using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h index 918da768db7..0e64f9b9356 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/nodes/upsample_trajectory_task.h @@ -51,9 +51,6 @@ class TESSERACT_TASK_COMPOSER_PLANNING_NODES_EXPORT UpsampleTrajectoryTask : pub static const std::string INOUT_PROGRAM_PORT; static const std::string INPUT_PROFILES_PORT; - // Optional - static const std::string INPUT_COMPOSITE_PROFILE_REMAPPING_PORT; - using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; using UPtr = std::unique_ptr; diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h index bf4dab5933d..c81f96e55b5 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/contact_check_profile.h @@ -32,23 +32,34 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { -struct ContactCheckProfile +struct ContactCheckProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; ContactCheckProfile(); - ContactCheckProfile(double longest_valid_segment_length, double contact_distance); - virtual ~ContactCheckProfile() = default; + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief The contact manager config */ tesseract_collision::CollisionCheckConfig config; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ContactCheckProfile) + #endif // TESSERACT_TASK_COMPOSER_CONTACT_CHECK_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h index f8e97dfd41c..2f85a566f87 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h @@ -32,9 +32,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct FixStateBoundsProfile +struct FixStateBoundsProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -47,7 +49,13 @@ struct FixStateBoundsProfile DISABLED }; - FixStateBoundsProfile(Settings mode = Settings::ALL) : mode(mode) {} + FixStateBoundsProfile(Settings mode = Settings::ALL); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief Sets which terms will be corrected */ Settings mode; @@ -60,7 +68,14 @@ struct FixStateBoundsProfile /** @brief Amount to increase the lower bounds before clamping limits. Should be > 1 */ double lower_bounds_reduction{ std::numeric_limits::epsilon() }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateBoundsProfile) + #endif // TESSERACT_TASK_COMPOSER_FIX_STATE_BOUNDS_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h index 41985894711..7f37d1536f4 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/fix_state_collision_profile.h @@ -33,10 +33,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { -struct FixStateCollisionProfile +struct FixStateCollisionProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; @@ -60,11 +61,13 @@ struct FixStateCollisionProfile RANDOM_SAMPLER }; - FixStateCollisionProfile(Settings mode = Settings::ALL) : mode(mode) - { - collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST; - collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; - } + FixStateCollisionProfile(Settings mode = Settings::ALL); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief Sets which terms will be corrected */ Settings mode; @@ -81,6 +84,14 @@ struct FixStateCollisionProfile /** @brief Number of sampling attempts if TrajOpt correction fails*/ int sampling_attempts{ 100 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::FixStateCollisionProfile) + #endif // TESSERACT_TASK_COMPOSER_FIX_STATE_COLLISION_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h index 4d2477ebf37..dcb7bc4301f 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h @@ -31,26 +31,37 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct IterativeSplineParameterizationProfile +struct IterativeSplineParameterizationProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - IterativeSplineParameterizationProfile() = default; - IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - { - } + IterativeSplineParameterizationProfile(); + IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ double max_velocity_scaling_factor{ 1.0 }; /** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */ double max_acceleration_scaling_factor{ 1.0 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::IterativeSplineParameterizationProfile) + #endif // TESSERACT_TASK_COMPOSER_ITERATIVE_SPLINE_PARAMETERIZATION_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h index c2ccb301e04..adfd5d9ec8e 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/min_length_profile.h @@ -33,18 +33,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct MinLengthProfile +struct MinLengthProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - MinLengthProfile() = default; - MinLengthProfile(long min_length) : min_length(min_length) {} + MinLengthProfile(); + MinLengthProfile(long min_length); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); long min_length{ 10 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::MinLengthProfile) + #endif // TESSERACT_TASK_COMPOSER_MIN_LENGTH_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h index 5f8e5773b9d..e1b2addc780 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/profile_switch_profile.h @@ -29,17 +29,32 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct ProfileSwitchProfile +struct ProfileSwitchProfile : public Profile { - ProfileSwitchProfile(int return_value = 1) : return_value(return_value) {} - using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + ProfileSwitchProfile(int return_value = 1); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); + int return_value; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::ProfileSwitchProfile) + #endif // TESSERACT_TASK_COMPOSER_PROFILE_SWITCH_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h index 265dd5796a5..8b99bce65b3 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h @@ -29,24 +29,26 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct RuckigTrajectorySmoothingCompositeProfile +struct RuckigTrajectorySmoothingCompositeProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingCompositeProfile() = default; + RuckigTrajectorySmoothingCompositeProfile(); RuckigTrajectorySmoothingCompositeProfile(double duration_extension_fraction, double max_duration_extension_factor, double max_velocity_scaling_factor, - double max_acceleration_scaling_factor) - : duration_extension_fraction(duration_extension_fraction) - , max_duration_extension_factor(max_duration_extension_factor) - , max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - { - } + double max_acceleration_scaling_factor); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief duration_extension_fraction The amount to scale the trajectory each time */ double duration_extension_fraction{ 1.1 }; @@ -64,17 +66,19 @@ struct RuckigTrajectorySmoothingCompositeProfile double max_jerk_scaling_factor{ 1.0 }; }; -struct RuckigTrajectorySmoothingMoveProfile +struct RuckigTrajectorySmoothingMoveProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - RuckigTrajectorySmoothingMoveProfile() = default; - RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor) - : max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - { - } + RuckigTrajectorySmoothingMoveProfile(); + RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief max_velocity_scaling_factor The max velocity scaling factor passed to the solver */ double max_velocity_scaling_factor{ 1.0 }; @@ -84,7 +88,14 @@ struct RuckigTrajectorySmoothingMoveProfile /** @brief max_jerk_scaling_factor The max jerk scaling factor passed to the solver */ double max_jerk_scaling_factor{ 1.0 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) + #endif // TESSERACT_TASK_COMPOSER_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h index 22a1d40b4a8..afc29a36774 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h @@ -32,26 +32,27 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct TimeOptimalParameterizationProfile +struct TimeOptimalParameterizationProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - TimeOptimalParameterizationProfile() = default; + TimeOptimalParameterizationProfile(); TimeOptimalParameterizationProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, double max_jerk_scaling_factor, double path_tolerance, - double min_angle_change) - : max_velocity_scaling_factor(max_velocity_scaling_factor) - , max_acceleration_scaling_factor(max_acceleration_scaling_factor) - , max_jerk_scaling_factor(max_jerk_scaling_factor) - , path_tolerance(path_tolerance) - , min_angle_change(min_angle_change) - { - } + double min_angle_change); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); /** @brief The max velocity scaling factor passed to the solver. Default: 1.0*/ double max_velocity_scaling_factor{ 1.0 }; @@ -67,7 +68,15 @@ struct TimeOptimalParameterizationProfile /** @brief At least one joint must change by greater than this amount for the point to be added. Default: 0.001*/ double min_angle_change{ 0.001 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::TimeOptimalParameterizationProfile) + #endif // TIME_OPTIMAL_PARAMETERIZATION_PROFILE_H diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h index df940e7f9fa..6bd56752aea 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h @@ -30,21 +30,33 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include + namespace tesseract_planning { -struct UpsampleTrajectoryProfile +struct UpsampleTrajectoryProfile : public Profile { using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - UpsampleTrajectoryProfile() = default; - UpsampleTrajectoryProfile(double longest_valid_segment_length) - : longest_valid_segment_length(longest_valid_segment_length) - { - } + UpsampleTrajectoryProfile(); + UpsampleTrajectoryProfile(double longest_valid_segment_length); + + /** + * @brief A utility function for getting profile ID + * @return The profile ID used when storing in profile dictionary + */ + static std::size_t getStaticKey(); double longest_valid_segment_length{ 0.1 }; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::UpsampleTrajectoryProfile) + #endif // TESSERACT_TASK_COMPOSER_UPSAMPLE_TRAJECTORY_PROFILE_H 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 909a496825d..af895391bb9 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 @@ -63,7 +63,6 @@ const std::string ContinuousContactCheckTask::INPUT_PROFILES_PORT = "profiles"; // Optional const std::string ContinuousContactCheckTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string ContinuousContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; const std::string ContinuousContactCheckTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results"; ContinuousContactCheckTask::ContinuousContactCheckTask() @@ -99,7 +98,6 @@ TaskComposerNodePorts ContinuousContactCheckTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE; return ports; @@ -144,14 +142,10 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto default_profile = std::make_shared(); default_profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - auto cur_composite_profile = getProfile(ns_, profile, *profiles, default_profile); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = getProfile(ns_, ci.getProfile(ns_), *profiles, default_profile); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input_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 99c4f7d558f..eb259369f84 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 @@ -63,7 +63,6 @@ const std::string DiscreteContactCheckTask::INPUT_PROFILES_PORT = "profiles"; // Optional const std::string DiscreteContactCheckTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string DiscreteContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; const std::string DiscreteContactCheckTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results"; DiscreteContactCheckTask::DiscreteContactCheckTask() @@ -99,7 +98,6 @@ TaskComposerNodePorts DiscreteContactCheckTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE; return ports; @@ -142,13 +140,9 @@ std::unique_ptr DiscreteContactCheckTask::runImpl(TaskComp // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, ci.getProfile(ns_), *profiles, std::make_shared()); // Get state solver tesseract_common::ManipulatorInfo manip_info = ci.getManipulatorInfo().getCombined(input_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 3202ee2cef4..2da94de1bda 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 @@ -55,7 +55,6 @@ const std::string FixStateBoundsTask::INPUT_PROFILES_PORT = "profiles"; // Optional const std::string FixStateBoundsTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string FixStateBoundsTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; FixStateBoundsTask::FixStateBoundsTask() : TaskComposerTask("FixStateBoundsTask", FixStateBoundsTask::ports(), true) {} FixStateBoundsTask::FixStateBoundsTask(std::string name, @@ -88,7 +87,6 @@ TaskComposerNodePorts FixStateBoundsTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; @@ -132,7 +130,6 @@ std::unique_ptr FixStateBoundsTask::runImpl(TaskComposerCo input_manip_info = manip_info_poly.as(); auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); auto& ci = input_data_poly.as(); ci.setManipulatorInfo(ci.getManipulatorInfo().getCombined(input_manip_info)); const tesseract_common::ManipulatorInfo& manip_info = ci.getManipulatorInfo(); @@ -140,11 +137,8 @@ std::unique_ptr FixStateBoundsTask::runImpl(TaskComposerCo auto limits = joint_group->getLimits(); // Get Composite Profile - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, ci.getProfile(ns_), *profiles, std::make_shared()); 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 edbb07b212e..6f4c172dbd4 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 @@ -66,7 +66,6 @@ const std::string FixStateCollisionTask::INPUT_PROFILES_PORT = "profiles"; // Optional const std::string FixStateCollisionTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string FixStateCollisionTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; const std::string FixStateCollisionTask::OUTPUT_CONTACT_RESULTS_PORT = "contact_results"; bool stateInCollision(const Eigen::Ref& start_pos, @@ -382,7 +381,6 @@ TaskComposerNodePorts FixStateCollisionTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; ports.output_optional[OUTPUT_CONTACT_RESULTS_PORT] = TaskComposerNodePorts::SINGLE; @@ -429,13 +427,9 @@ std::unique_ptr FixStateCollisionTask::runImpl(TaskCompose // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); - auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = getProfile( + ns_, ci.getProfile(ns_), *profiles, std::make_shared()); std::vector contact_results; 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 936d983e21f..c071fcba392 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 @@ -56,9 +56,6 @@ const std::string IterativeSplineParameterizationTask::INPUT_PROFILES_PORT = "pr // Optional const std::string IterativeSplineParameterizationTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string IterativeSplineParameterizationTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_" - "remapping"; -const std::string IterativeSplineParameterizationTask::INPUT_MOVE_PROFILE_REMAPPING_PORT = "move_profile_remapping"; IterativeSplineParameterizationTask::IterativeSplineParameterizationTask() : TaskComposerTask("IterativeSplineParameterizationTask", IterativeSplineParameterizationTask::ports(), true) @@ -108,8 +105,6 @@ TaskComposerNodePorts IterativeSplineParameterizationTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_MOVE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; @@ -160,13 +155,9 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); - auto move_profile_remapping_poly = getData(*context.data_storage, INPUT_MOVE_PROFILE_REMAPPING_PORT, false); std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = getProfile( - ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, ci.getProfile(ns_), *profiles, std::make_shared()); // Create data structures for checking for plan profile overrides auto flattened = ci.flatten(moveFilter); @@ -195,13 +186,10 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, 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(ns_, profile, move_profile_remapping_poly); + // Check for remapping of the plan profil auto cur_move_profile = getProfile( - ns_, move_profile, *profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(ns_, profile, cur_move_profile, mi.profile_overrides); + ns_, mi.getProfile(ns_), *profiles, std::make_shared()); // If there is a move profile associated with it, override the parameters if (cur_move_profile) 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 5fe97069635..1c090e2d1a9 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -57,9 +57,6 @@ const std::string MinLengthTask::INOUT_PROGRAM_PORT = "program"; const std::string MinLengthTask::INPUT_ENVIRONMENT_PORT = "environment"; const std::string MinLengthTask::INPUT_PROFILES_PORT = "profiles"; -// Optional -const std::string MinLengthTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; - MinLengthTask::MinLengthTask() : TaskComposerTask("MinLengthTask", MinLengthTask::ports(), false) {} MinLengthTask::MinLengthTask(std::string name, std::string input_program_key, @@ -90,8 +87,6 @@ TaskComposerNodePorts MinLengthTask::ports() ports.input_required[INPUT_ENVIRONMENT_PORT] = TaskComposerNodePorts::SINGLE; ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; - ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; return ports; @@ -129,14 +124,10 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); const auto& ci = input_data_poly.as(); long cnt = ci.getMoveInstructionCount(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, ci.getProfile(ns_), *profiles, std::make_shared()); if (cnt < cur_composite_profile->min_length) { @@ -146,7 +137,6 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Fill out request and response PlannerRequest request; request.instructions = ci; - request.env_state = env->getState(); request.env = env; // Set up planner @@ -156,11 +146,10 @@ std::unique_ptr MinLengthTask::runImpl(TaskComposerContext // Create profile dictionary auto simple_profiles = std::make_shared(); - simple_profiles->addProfile(planner.getName(), ci.getProfile(), profile); + simple_profiles->addProfile(planner.getName(), ci.getProfile(), profile); auto flat = ci.flatten(&moveFilter); for (const auto& i : flat) - simple_profiles->addProfile( - planner.getName(), i.get().as().getProfile(), profile); + simple_profiles->addProfile(planner.getName(), i.get().as().getProfile(), profile); // Assign profile dictionary request.profiles = simple_profiles; 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 a74dac807d9..a6d27093ba4 100644 --- a/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/profile_switch_task.cpp @@ -48,9 +48,6 @@ namespace tesseract_planning const std::string ProfileSwitchTask::INPUT_PROGRAM_PORT = "program"; const std::string ProfileSwitchTask::INPUT_PROFILES_PORT = "profiles"; -// Optional -const std::string ProfileSwitchTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; - ProfileSwitchTask::ProfileSwitchTask() : TaskComposerTask("ProfileSwitchTask", ProfileSwitchTask::ports(), true) {} ProfileSwitchTask::ProfileSwitchTask(std::string name, std::string input_program_key, @@ -75,7 +72,6 @@ TaskComposerNodePorts ProfileSwitchTask::ports() TaskComposerNodePorts ports; ports.input_required[INPUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; return ports; } @@ -99,13 +95,9 @@ std::unique_ptr ProfileSwitchTask::runImpl(TaskComposerCon // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + getProfile(ns_, ci.getProfile(ns_), *profiles, std::make_shared()); // 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 19f118ef917..5a01519f3e4 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 @@ -56,8 +56,6 @@ const std::string RuckigTrajectorySmoothingTask::INPUT_PROFILES_PORT = "profiles // Optional const std::string RuckigTrajectorySmoothingTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string RuckigTrajectorySmoothingTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; -const std::string RuckigTrajectorySmoothingTask::INPUT_MOVE_PROFILE_REMAPPING_PORT = "move_profile_remapping"; RuckigTrajectorySmoothingTask::RuckigTrajectorySmoothingTask() : TaskComposerTask("RuckigTrajectorySmoothingTask", RuckigTrajectorySmoothingTask::ports(), true) @@ -93,8 +91,6 @@ TaskComposerNodePorts RuckigTrajectorySmoothingTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_MOVE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; @@ -145,13 +141,8 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); - auto move_profile_remapping_poly = getData(*context.data_storage, INPUT_MOVE_PROFILE_REMAPPING_PORT, false); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = getProfile( - ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, ci.getProfile(ns_), *profiles, std::make_shared()); RuckigTrajectorySmoothing solver(cur_composite_profile->duration_extension_fraction, cur_composite_profile->max_duration_extension_factor); @@ -184,13 +175,10 @@ RuckigTrajectorySmoothingTask::runImpl(TaskComposerContext& context, OptionalTas 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(ns_, profile, move_profile_remapping_poly); auto cur_move_profile = getProfile( - ns_, move_profile, *profiles, std::make_shared()); - // cur_move_profile = applyProfileOverrides(ns_, profile, cur_move_profile, mi.profile_overrides); + ns_, mi.getProfile(ns_), *profiles, std::make_shared()); // If there is a move profile associated with it, override the parameters if (cur_move_profile) 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 3704292f0b9..309ea0eb42c 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 @@ -62,9 +62,6 @@ const std::string TimeOptimalParameterizationTask::INPUT_PROFILES_PORT = "profil // Optional const std::string TimeOptimalParameterizationTask::INPUT_MANIP_INFO_PORT = "manip_info"; -const std::string TimeOptimalParameterizationTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_" - "remapping"; -const std::string TimeOptimalParameterizationTask::INPUT_MOVE_PROFILE_REMAPPING_PORT = "move_profile_remapping"; TimeOptimalParameterizationTask::TimeOptimalParameterizationTask() : TaskComposerTask("TimeOptimalParameterizationTask", TimeOptimalParameterizationTask::ports(), true) @@ -100,8 +97,6 @@ TaskComposerNodePorts TimeOptimalParameterizationTask::ports() ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; ports.input_optional[INPUT_MANIP_INFO_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_MOVE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; @@ -151,12 +146,8 @@ TimeOptimalParameterizationTask::runImpl(TaskComposerContext& context, OptionalT // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); auto cur_composite_profile = getProfile( - ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + ns_, ci.getProfile(ns_), *profiles, std::make_shared()); // 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 927cfd6abc8..17276da613e 100644 --- a/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/upsample_trajectory_task.cpp @@ -52,9 +52,6 @@ namespace tesseract_planning const std::string UpsampleTrajectoryTask::INOUT_PROGRAM_PORT = "program"; const std::string UpsampleTrajectoryTask::INPUT_PROFILES_PORT = "profiles"; -// Optional -const std::string UpsampleTrajectoryTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT = "composite_profile_remapping"; - UpsampleTrajectoryTask::UpsampleTrajectoryTask() : TaskComposerTask("UpsampleTrajectoryTask", UpsampleTrajectoryTask::ports(), false) { @@ -84,7 +81,6 @@ TaskComposerNodePorts UpsampleTrajectoryTask::ports() TaskComposerNodePorts ports; ports.input_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; ports.input_required[INPUT_PROFILES_PORT] = TaskComposerNodePorts::SINGLE; - ports.input_optional[INPUT_COMPOSITE_PROFILE_REMAPPING_PORT] = TaskComposerNodePorts::SINGLE; ports.output_required[INOUT_PROGRAM_PORT] = TaskComposerNodePorts::SINGLE; return ports; } @@ -107,13 +103,9 @@ std::unique_ptr UpsampleTrajectoryTask::runImpl(TaskCompos // Get Composite Profile auto profiles = getData(*context.data_storage, INPUT_PROFILES_PORT).as>(); - auto composite_profile_remapping_poly = getData(*context.data_storage, INPUT_COMPOSITE_PROFILE_REMAPPING_PORT, false); const auto& ci = input_data_poly.as(); - std::string profile = ci.getProfile(); - profile = getProfileString(ns_, profile, composite_profile_remapping_poly); - auto cur_composite_profile = - getProfile(ns_, profile, *profiles, std::make_shared()); - cur_composite_profile = applyProfileOverrides(ns_, profile, cur_composite_profile, ci.getProfileOverrides()); + auto cur_composite_profile = getProfile( + ns_, ci.getProfile(ns_), *profiles, std::make_shared()); assert(cur_composite_profile->longest_valid_segment_length > 0); InstructionPoly start_instruction; diff --git a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp index e7a7c98bedb..5288c3e0d30 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -27,15 +27,20 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { ContactCheckProfile::ContactCheckProfile() : ContactCheckProfile(0.05, 0) {} ContactCheckProfile::ContactCheckProfile(double longest_valid_segment_length, double contact_distance) + : Profile(ContactCheckProfile::getStaticKey()) { config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; config.longest_valid_segment_length = longest_valid_segment_length; @@ -49,4 +54,18 @@ ContactCheckProfile::ContactCheckProfile(double longest_valid_segment_length, do config.longest_valid_segment_length = 0.05; } } + +std::size_t ContactCheckProfile::getStaticKey() { return std::type_index(typeid(ContactCheckProfile)).hash_code(); } + +template +void ContactCheckProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(config); +} + } // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ContactCheckProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ContactCheckProfile) diff --git a/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp new file mode 100644 index 00000000000..20d830f90a5 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp @@ -0,0 +1,53 @@ +/** + * @file fix_state_bounds_profile.cpp + * @brief Profile for process that pushes plan instructions back within joint limits + * + * @author Matthew Powelson + * @date August 31. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include + +namespace tesseract_planning +{ +FixStateBoundsProfile::FixStateBoundsProfile(Settings mode) : Profile(FixStateBoundsProfile::getStaticKey()), mode(mode) +{ +} + +std::size_t FixStateBoundsProfile::getStaticKey() { return std::type_index(typeid(FixStateBoundsProfile)).hash_code(); } + +template +void FixStateBoundsProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(mode); + ar& BOOST_SERIALIZATION_NVP(max_deviation_global); + ar& BOOST_SERIALIZATION_NVP(upper_bounds_reduction); + ar& BOOST_SERIALIZATION_NVP(lower_bounds_reduction); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateBoundsProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateBoundsProfile) diff --git a/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp new file mode 100644 index 00000000000..3ee61e36348 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp @@ -0,0 +1,63 @@ +/** + * @file fix_state_collision_profile.cpp + * @brief Profile for process that pushes plan instructions to be out of collision + * + * @author Matthew Powelson + * @date August 31. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include + +namespace tesseract_planning +{ +FixStateCollisionProfile::FixStateCollisionProfile(Settings mode) + : Profile(FixStateCollisionProfile::getStaticKey()), mode(mode) +{ + collision_check_config.contact_request.type = tesseract_collision::ContactTestType::FIRST; + collision_check_config.type = tesseract_collision::CollisionEvaluatorType::DISCRETE; +} + +std::size_t FixStateCollisionProfile::getStaticKey() +{ + return std::type_index(typeid(FixStateCollisionProfile)).hash_code(); +} + +template +void FixStateCollisionProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(mode); + ar& BOOST_SERIALIZATION_NVP(correction_workflow); + ar& BOOST_SERIALIZATION_NVP(jiggle_factor); + ar& BOOST_SERIALIZATION_NVP(collision_check_config); + ar& BOOST_SERIALIZATION_NVP(sampling_attempts); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::FixStateCollisionProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::FixStateCollisionProfile) diff --git a/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp new file mode 100644 index 00000000000..15dfe2a2db1 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp @@ -0,0 +1,64 @@ +/** + * @file iterative_spline_parameterization_profile.cpp + * @brief Profile for iterative spline time parameterization + * + * @author Levi Armstrong + * @date August 11. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +IterativeSplineParameterizationProfile::IterativeSplineParameterizationProfile() + : Profile(IterativeSplineParameterizationProfile::getStaticKey()) +{ +} + +IterativeSplineParameterizationProfile::IterativeSplineParameterizationProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor) + : Profile(IterativeSplineParameterizationProfile::getStaticKey()) + , max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) +{ +} + +std::size_t IterativeSplineParameterizationProfile::getStaticKey() +{ + return std::type_index(typeid(IterativeSplineParameterizationProfile)).hash_code(); +} + +template +void IterativeSplineParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::IterativeSplineParameterizationProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::IterativeSplineParameterizationProfile) diff --git a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp new file mode 100644 index 00000000000..8036add1450 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -0,0 +1,53 @@ +/** + * @file min_length_profile.cpp + * @brief Profile for task that processing the program so it meets a minimum length. Planners like trajopt + * need at least the user defined number of states in the trajectory to perform velocity, acceleration and jerk + * smoothing. + * + * @author Levi Armstrong + * @date November 2. 2020 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +MinLengthProfile::MinLengthProfile() : Profile(MinLengthProfile::getStaticKey()) {} +MinLengthProfile::MinLengthProfile(long min_length) : Profile(MinLengthProfile::getStaticKey()), min_length(min_length) +{ +} + +std::size_t MinLengthProfile::getStaticKey() { return std::type_index(typeid(MinLengthProfile)).hash_code(); } + +template +void MinLengthProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(min_length); +} +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::MinLengthProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::MinLengthProfile) diff --git a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp new file mode 100644 index 00000000000..f6ce5d1e071 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -0,0 +1,50 @@ +/** + * @file profile_switch_profile.h + * @brief Profile for task that returns a value based on the profile + * + * @author Matthew Powelson + * @date October 26. 2020 + * @version TODO + * @bug No known bugs + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +ProfileSwitchProfile::ProfileSwitchProfile(int return_value) + : Profile(ProfileSwitchProfile::getStaticKey()), return_value(return_value) +{ +} + +std::size_t ProfileSwitchProfile::getStaticKey() { return std::type_index(typeid(ProfileSwitchProfile)).hash_code(); } + +template +void ProfileSwitchProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(return_value); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::ProfileSwitchProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::ProfileSwitchProfile) diff --git a/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp new file mode 100644 index 00000000000..12113f0cf6f --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -0,0 +1,85 @@ +/** + * @file ruckig_trajectory_smoothing_profile.cpp + * @brief Leveraging Ruckig to smooth trajectory + * + * @author Levi Armstrong + * @date July 27, 2022 + * @version TODO + * @bug No known bugs + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +RuckigTrajectorySmoothingCompositeProfile::RuckigTrajectorySmoothingCompositeProfile() + : Profile(RuckigTrajectorySmoothingCompositeProfile::getStaticKey()) +{ +} + +RuckigTrajectorySmoothingCompositeProfile::RuckigTrajectorySmoothingCompositeProfile( + double duration_extension_fraction, + double max_duration_extension_factor, + double max_velocity_scaling_factor, + double max_acceleration_scaling_factor) + : Profile(RuckigTrajectorySmoothingCompositeProfile::getStaticKey()) + , duration_extension_fraction(duration_extension_fraction) + , max_duration_extension_factor(max_duration_extension_factor) + , max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) +{ +} + +std::size_t RuckigTrajectorySmoothingCompositeProfile::getStaticKey() +{ + return std::type_index(typeid(RuckigTrajectorySmoothingCompositeProfile)).hash_code(); +} + +RuckigTrajectorySmoothingMoveProfile::RuckigTrajectorySmoothingMoveProfile() + : Profile(RuckigTrajectorySmoothingMoveProfile::getStaticKey()) +{ +} +RuckigTrajectorySmoothingMoveProfile::RuckigTrajectorySmoothingMoveProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor) + : Profile(RuckigTrajectorySmoothingMoveProfile::getStaticKey()) + , max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) +{ +} + +std::size_t RuckigTrajectorySmoothingMoveProfile::getStaticKey() +{ + return std::type_index(typeid(RuckigTrajectorySmoothingMoveProfile)).hash_code(); +} + +template +void RuckigTrajectorySmoothingMoveProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) diff --git a/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp new file mode 100644 index 00000000000..63b2c838f35 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp @@ -0,0 +1,74 @@ +/** + * @file time_optimal_parameterization_profile.cpp + * @brief Profile for TOTG process + * + * @author Levi Armstrong + * @author Matthew Powelson + * @date Jan 22, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2020, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +TimeOptimalParameterizationProfile::TimeOptimalParameterizationProfile() + : Profile(TimeOptimalParameterizationProfile::getStaticKey()) +{ +} + +TimeOptimalParameterizationProfile::TimeOptimalParameterizationProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, + double max_jerk_scaling_factor, + double path_tolerance, + double min_angle_change) + : Profile(TimeOptimalParameterizationProfile::getStaticKey()) + , max_velocity_scaling_factor(max_velocity_scaling_factor) + , max_acceleration_scaling_factor(max_acceleration_scaling_factor) + , max_jerk_scaling_factor(max_jerk_scaling_factor) + , path_tolerance(path_tolerance) + , min_angle_change(min_angle_change) +{ +} + +std::size_t TimeOptimalParameterizationProfile::getStaticKey() +{ + return std::type_index(typeid(TimeOptimalParameterizationProfile)).hash_code(); +} + +template +void TimeOptimalParameterizationProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(path_tolerance); + ar& BOOST_SERIALIZATION_NVP(min_angle_change); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::TimeOptimalParameterizationProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::TimeOptimalParameterizationProfile) diff --git a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp new file mode 100644 index 00000000000..514400a7636 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -0,0 +1,56 @@ +/** + * @file upsample_trajectory_profile.cpp + * + * @author Levi Armstrong + * @date December 15, 2021 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2021, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +namespace tesseract_planning +{ +UpsampleTrajectoryProfile::UpsampleTrajectoryProfile() : Profile(UpsampleTrajectoryProfile::getStaticKey()) {} + +UpsampleTrajectoryProfile::UpsampleTrajectoryProfile(double longest_valid_segment_length) + : Profile(UpsampleTrajectoryProfile::getStaticKey()), longest_valid_segment_length(longest_valid_segment_length) +{ +} + +std::size_t UpsampleTrajectoryProfile::getStaticKey() +{ + return std::type_index(typeid(UpsampleTrajectoryProfile)).hash_code(); +} + +template +void UpsampleTrajectoryProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(longest_valid_segment_length); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::UpsampleTrajectoryProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::UpsampleTrajectoryProfile) diff --git a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp index 02c31f81bd6..55c8d9def93 100644 --- a/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp +++ b/tesseract_task_composer/test/fix_state_bounds_task_unit.cpp @@ -150,7 +150,7 @@ TEST_F(FixStateBoundsTaskUnit, stateInBounds) // NOLINT state_bounds_profile->upper_bounds_reduction = 1e-3; auto profiles = std::make_shared(); - profiles->addProfile(FIX_STATE_BOUNDS_TASK_NAME, DEFAULT_PROFILE_KEY, state_bounds_profile); + profiles->addProfile(FIX_STATE_BOUNDS_TASK_NAME, DEFAULT_PROFILE_KEY, state_bounds_profile); Eigen::VectorXd mid_state = joint_limits.col(0) + (joint_limits.col(1) - joint_limits.col(0)) / 2.; { // All are valid diff --git a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index bd046c08306..4ced5c32b7c 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -95,19 +95,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask program: input_data environment: environment profiles: profiles - manip_info: manip_info - composite_profile_remapping: composite_profile_remapping)"; + manip_info: manip_info)"; YAML::Node config = YAML::Load(str); ContinuousContactCheckTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 5); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(ContinuousContactCheckTask::INPUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(ContinuousContactCheckTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(ContinuousContactCheckTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(ContinuousContactCheckTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(ContinuousContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 0); EXPECT_EQ(task.getOutboundEdges().size(), 0); EXPECT_EQ(task.getInboundEdges().size(), 0); @@ -229,8 +226,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerContinuousContactCheckTask auto profile = std::make_unique(); profile->config.contact_manager_config = tesseract_collision::ContactManagerConfig(1.5); profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS; - profiles->addProfile( - "TaskComposerContinuousContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); + profiles->addProfile("TaskComposerContinuousContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); @@ -273,19 +269,16 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe program: input_data environment: environment profiles: profiles - manip_info: manip_info - composite_profile_remapping: composite_profile_remapping)"; + manip_info: manip_info)"; YAML::Node config = YAML::Load(str); DiscreteContactCheckTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 5); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(DiscreteContactCheckTask::INPUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(DiscreteContactCheckTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(DiscreteContactCheckTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(DiscreteContactCheckTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(DiscreteContactCheckTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 0); EXPECT_EQ(task.getOutboundEdges().size(), 0); EXPECT_EQ(task.getInboundEdges().size(), 0); @@ -403,8 +396,7 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerDiscreteContactCheckTaskTe auto profile = std::make_unique(); profile->config.contact_manager_config = tesseract_collision::ContactManagerConfig(1.5); profile->config.type = tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE; - profiles->addProfile( - "TaskComposerDiscreteContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); + profiles->addProfile("TaskComposerDiscreteContactCheckTaskTests", DEFAULT_PROFILE_KEY, std::move(profile)); auto data = std::make_unique(); data->setData("input_data", test_suite::jointInterpolateExampleProgramABB()); @@ -1113,20 +1105,17 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateBoundsTaskTests) environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping outputs: program: output_data)"; YAML::Node config = YAML::Load(str); FixStateBoundsTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 5); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(FixStateBoundsTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(FixStateBoundsTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(FixStateBoundsTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(FixStateBoundsTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(FixStateBoundsTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(FixStateBoundsTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -1288,20 +1277,17 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerFixStateCollisionTaskTests environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping outputs: program: output_data)"; YAML::Node config = YAML::Load(str); FixStateCollisionTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 5); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(FixStateCollisionTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(FixStateCollisionTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(FixStateCollisionTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(FixStateCollisionTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(FixStateCollisionTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(FixStateCollisionTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -1439,17 +1425,14 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerProfileSwitchTaskTests) / conditional: true inputs: program: input_data - profiles: profiles - composite_profile_remapping: composite_profile_remapping)"; + profiles: profiles)"; YAML::Node config = YAML::Load(str); ProfileSwitchTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 3); + EXPECT_EQ(task.getInputKeys().size(), 2); EXPECT_EQ(task.getInputKeys().get(ProfileSwitchTask::INPUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(ProfileSwitchTask::INPUT_PROFILES_PORT), "profiles"); - EXPECT_EQ(task.getInputKeys().get(ProfileSwitchTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 0); EXPECT_EQ(task.getOutboundEdges().size(), 0); EXPECT_EQ(task.getInboundEdges().size(), 0); @@ -1882,18 +1865,15 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerUpsampleTrajectoryTaskTest inputs: program: input_data profiles: profiles - composite_profile_remapping: composite_profile_remapping outputs: program: output_data)"; YAML::Node config = YAML::Load(str); UpsampleTrajectoryTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 3); + EXPECT_EQ(task.getInputKeys().size(), 2); EXPECT_EQ(task.getInputKeys().get(UpsampleTrajectoryTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(UpsampleTrajectoryTask::INPUT_PROFILES_PORT), "profiles"); - EXPECT_EQ(task.getInputKeys().get(UpsampleTrajectoryTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(UpsampleTrajectoryTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -2009,8 +1989,6 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data add_points: true)"; @@ -2018,15 +1996,11 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerIterativeSplineParameteriz IterativeSplineParameterizationTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 6); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); - EXPECT_EQ(task.getInputKeys().get(IterativeSplineParameterizationTask::INPUT_MOVE_PROFILE_REMAPPING_PORT), - "move_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(IterativeSplineParameterizationTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -2202,23 +2176,17 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerTimeOptimalParameterizatio environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data)"; YAML::Node config = YAML::Load(str); TimeOptimalParameterizationTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 6); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); - EXPECT_EQ(task.getInputKeys().get(TimeOptimalParameterizationTask::INPUT_MOVE_PROFILE_REMAPPING_PORT), - "move_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(TimeOptimalParameterizationTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -2396,23 +2364,17 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerRuckigTrajectorySmoothingT environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data)"; YAML::Node config = YAML::Load(str); RuckigTrajectorySmoothingTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 6); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); - EXPECT_EQ(task.getInputKeys().get(RuckigTrajectorySmoothingTask::INPUT_MOVE_PROFILE_REMAPPING_PORT), - "move_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(RuckigTrajectorySmoothingTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0); @@ -2597,8 +2559,6 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / environment: environment profiles: profiles manip_info: manip_info - composite_profile_remapping: composite_profile_remapping - move_profile_remapping: move_profile_remapping outputs: program: output_data format_result_as_input: false)"; @@ -2606,15 +2566,11 @@ TEST_F(TesseractTaskComposerPlanningUnit, TaskComposerMotionPlannerTaskTests) / MotionPlannerTask task("abc", config["config"], factory); EXPECT_EQ(task.getName(), "abc"); EXPECT_EQ(task.isConditional(), true); - EXPECT_EQ(task.getInputKeys().size(), 6); + EXPECT_EQ(task.getInputKeys().size(), 4); EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INOUT_PROGRAM_PORT), "input_data"); EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INPUT_ENVIRONMENT_PORT), "environment"); EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INPUT_PROFILES_PORT), "profiles"); EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INPUT_MANIP_INFO_PORT), "manip_info"); - EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INPUT_COMPOSITE_PROFILE_REMAPPING_PORT), - "composite_profile_remapping"); - EXPECT_EQ(task.getInputKeys().get(MotionPlannerTask::INPUT_MOVE_PROFILE_REMAPPING_PORT), - "move_profile_remapping"); EXPECT_EQ(task.getOutputKeys().size(), 1); EXPECT_EQ(task.getOutputKeys().get(MotionPlannerTask::INOUT_PROGRAM_PORT), "output_data"); EXPECT_EQ(task.getOutboundEdges().size(), 0);