From f141bddd872ebdb635f92fb7c25af98215148b8d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 3 Dec 2024 11:36:59 -0600 Subject: [PATCH 01/13] Update dependencies.repos (#536) --- .github/workflows/windows_dependencies.repos | 2 +- dependencies.repos | 4 ---- dependencies_unstable.repos | 4 ---- 3 files changed, 1 insertion(+), 9 deletions(-) diff --git a/.github/workflows/windows_dependencies.repos b/.github/workflows/windows_dependencies.repos index 429bd5c8f73..abff024a18b 100644 --- a/.github/workflows/windows_dependencies.repos +++ b/.github/workflows/windows_dependencies.repos @@ -1,7 +1,7 @@ - git: local-name: ros_industrial_cmake_boilerplate uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git - version: 0.6.2 + version: 0.6.3 - git: local-name: tesseract uri: https://github.com/ros-industrial-consortium/tesseract.git diff --git a/dependencies.repos b/dependencies.repos index 24589553358..b48b296f4b5 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -1,7 +1,3 @@ -- git: - local-name: ros_industrial_cmake_boilerplate - uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git - version: 0.6.2 - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git diff --git a/dependencies_unstable.repos b/dependencies_unstable.repos index 6af679a1868..84602a0b796 100644 --- a/dependencies_unstable.repos +++ b/dependencies_unstable.repos @@ -1,7 +1,3 @@ -- git: - local-name: ros_industrial_cmake_boilerplate - uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git - version: master - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git From 61a73e4c3110166ad35281cedee4a3228d1bac7d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 4 Dec 2024 14:44:59 -0600 Subject: [PATCH 02/13] Add profile base class and update profile dictionary to not leverage template methods --- tesseract_command_language/CMakeLists.txt | 1 + .../include/tesseract_command_language/fwd.h | 1 + .../tesseract_command_language/profile.h | 68 +++++++ .../profile_dictionary.h | 176 ++++-------------- tesseract_command_language/src/profile.cpp | 46 +++++ .../src/profile_dictionary.cpp | 116 +++++++++++- .../src/basic_cartesian_example.cpp | 13 +- tesseract_examples/src/car_seat_example.cpp | 13 +- .../src/freespace_hybrid_example.cpp | 5 +- .../src/freespace_ompl_example.cpp | 2 +- .../src/glass_upright_example.cpp | 8 +- .../src/pick_and_place_example.cpp | 19 +- .../puzzle_piece_auxillary_axes_example.cpp | 15 +- .../src/puzzle_piece_example.cpp | 15 +- tesseract_motion_planners/core/CMakeLists.txt | 5 +- .../tesseract_motion_planners/planner_utils.h | 15 +- .../core/test/profile_dictionary_tests.cpp | 73 +++++--- .../descartes/CMakeLists.txt | 4 +- .../profile/descartes_default_plan_profile.h | 8 + .../descartes/profile/descartes_profile.h | 26 ++- .../descartes_default_plan_profile.cpp | 15 ++ .../src/profile/descartes_profile.cpp | 45 +++++ .../test/descartes_planner_tests.cpp | 6 +- tesseract_motion_planners/ompl/CMakeLists.txt | 1 + .../ompl/profile/ompl_default_plan_profile.h | 7 + .../ompl/profile/ompl_profile.h | 27 ++- .../src/profile/ompl_default_plan_profile.cpp | 12 ++ .../ompl/src/profile/ompl_profile.cpp | 48 +++++ .../ompl/test/ompl_planner_tests.cpp | 6 +- .../simple/CMakeLists.txt | 1 + ...e_planner_fixed_size_assign_plan_profile.h | 7 + .../simple_planner_fixed_size_plan_profile.h | 7 + .../simple_planner_lvs_no_ik_plan_profile.h | 7 + .../profile/simple_planner_lvs_plan_profile.h | 7 + .../simple/profile/simple_planner_profile.h | 50 +++-- .../simple/src/interpolation.cpp | 5 +- ...planner_fixed_size_assign_plan_profile.cpp | 13 ++ ...simple_planner_fixed_size_plan_profile.cpp | 13 ++ .../simple_planner_lvs_no_ik_plan_profile.cpp | 13 ++ .../simple_planner_lvs_plan_profile.cpp | 13 ++ .../src/profile/simple_planner_profile.cpp | 67 +++++++ .../trajopt/CMakeLists.txt | 1 + .../trajopt_default_composite_profile.h | 6 + .../profile/trajopt_default_plan_profile.h | 6 + .../profile/trajopt_default_solver_profile.h | 8 + .../trajopt/profile/trajopt_profile.h | 77 +++++--- .../trajopt_default_composite_profile.cpp | 12 ++ .../profile/trajopt_default_plan_profile.cpp | 12 ++ .../trajopt_default_solver_profile.cpp | 12 ++ .../trajopt/src/profile/trajopt_profile.cpp | 74 ++++++++ .../trajopt/test/trajopt_planner_tests.cpp | 32 ++-- .../trajopt_ifopt/CMakeLists.txt | 1 + .../trajopt_ifopt_default_composite_profile.h | 7 + .../trajopt_ifopt_default_plan_profile.h | 7 + .../trajopt_ifopt_default_solver_profile.h | 8 + .../profile/trajopt_ifopt_profile.h | 79 +++++--- ...rajopt_ifopt_default_composite_profile.cpp | 12 ++ .../trajopt_ifopt_default_plan_profile.cpp | 12 ++ .../trajopt_ifopt_default_solver_profile.cpp | 12 ++ .../src/profile/trajopt_ifopt_profile.cpp | 80 ++++++++ .../planning/CMakeLists.txt | 11 +- .../planning/profiles/contact_check_profile.h | 17 +- .../profiles/fix_state_bounds_profile.h | 19 +- .../profiles/fix_state_collision_profile.h | 23 ++- ...terative_spline_parameterization_profile.h | 25 ++- .../planning/profiles/min_length_profile.h | 21 ++- .../profiles/profile_switch_profile.h | 21 ++- .../ruckig_trajectory_smoothing_profile.h | 43 +++-- .../time_optimal_parameterization_profile.h | 29 ++- .../profiles/upsample_trajectory_profile.h | 24 ++- .../planning/src/nodes/min_length_task.cpp | 5 +- .../src/profiles/contact_check_profile.cpp | 17 ++ .../src/profiles/fix_state_bounds_profile.cpp | 49 +++++ .../profiles/fix_state_collision_profile.cpp | 56 ++++++ ...rative_spline_parameterization_profile.cpp | 62 ++++++ .../src/profiles/min_length_profile.cpp | 52 ++++++ .../src/profiles/profile_switch_profile.cpp | 49 +++++ .../ruckig_trajectory_smoothing_profile.cpp | 82 ++++++++ .../time_optimal_parameterization_profile.cpp | 69 +++++++ .../profiles/upsample_trajectory_profile.cpp | 55 ++++++ .../test/fix_state_bounds_task_unit.cpp | 2 +- .../tesseract_task_composer_planning_unit.cpp | 6 +- 82 files changed, 1804 insertions(+), 390 deletions(-) create mode 100644 tesseract_command_language/include/tesseract_command_language/profile.h create mode 100644 tesseract_command_language/src/profile.cpp create mode 100644 tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp create mode 100644 tesseract_motion_planners/ompl/src/profile/ompl_profile.cpp create mode 100644 tesseract_motion_planners/simple/src/profile/simple_planner_profile.cpp create mode 100644 tesseract_motion_planners/trajopt/src/profile/trajopt_profile.cpp create mode 100644 tesseract_motion_planners/trajopt_ifopt/src/profile/trajopt_ifopt_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/min_length_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp create mode 100644 tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp 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/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/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h new file mode 100644 index 00000000000..6d73f9de740 --- /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 id); + 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..d22194faa75 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -28,14 +28,12 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include #include #include -#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include namespace tesseract_planning @@ -50,13 +48,8 @@ namespace tesseract_planning 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 +57,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 +64,72 @@ 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, 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/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..e788657905b 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -25,19 +25,131 @@ */ #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, 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; + } + } +} + +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*/) { + ar& boost::serialization::make_nvp("profiles", profiles_); } } // namespace tesseract_planning 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/CMakeLists.txt b/tesseract_motion_planners/core/CMakeLists.txt index 1ee03a59fe1..949263a39a2 100644 --- a/tesseract_motion_planners/core/CMakeLists.txt +++ b/tesseract_motion_planners/core/CMakeLists.txt @@ -1,5 +1,8 @@ # Create interface for core -add_library(${PROJECT_NAME}_core src/planner.cpp src/types.cpp src/utils.cpp) +add_library(${PROJECT_NAME}_core + src/planner.cpp + src/types.cpp + src/utils.cpp) target_link_libraries( ${PROJECT_NAME}_core PUBLIC tesseract::tesseract_environment 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..9d0eb86da14 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 @@ -138,8 +138,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 +149,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()); } @@ -171,14 +172,14 @@ std::shared_ptr getProfile(const std::string& ns, template std::shared_ptr applyProfileOverrides(const std::string& ns, const std::string& profile, - const std::shared_ptr& nominal_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); + if (overrides->hasProfile(ProfileType::getStaticKey(), ns, profile)) + return std::static_pointer_cast(overrides->getProfile(ProfileType::getStaticKey(), ns, profile)); return nominal_profile; } 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/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index e7626c99338..097b694dcfb 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -11,7 +11,9 @@ add_library( src/serialize.cpp src/deserialize.cpp src/descartes_utils.cpp - src/profile/descartes_default_plan_profile.cpp) + src/profile/descartes_profile.cpp + src/profile/descartes_default_plan_profile.cpp +) target_link_libraries( ${PROJECT_NAME}_descartes PUBLIC ${PROJECT_NAME}_core 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..a52bdc9ebad 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 @@ -103,10 +103,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..b880b75f8d9 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,23 @@ 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()) {} + DescartesPlanProfile(const DescartesPlanProfile&) = delete; + DescartesPlanProfile& operator=(const DescartesPlanProfile&) = delete; + DescartesPlanProfile(DescartesPlanProfile&&) noexcept = delete; + DescartesPlanProfile& operator=(DescartesPlanProfile&&) noexcept = delete; + + /** + * @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 +78,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..f73cc5d87d4 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,8 @@ * limitations under the License. */ #include +#include +#include namespace tesseract_planning { @@ -31,4 +33,17 @@ 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); +} + } // 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..df74d6dccd9 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -145,7 +145,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // 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); @@ -261,7 +261,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); @@ -364,7 +364,7 @@ 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; 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/profile/ompl_default_plan_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h index b65cfbef016..fbb7e9f4c71 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 @@ -160,6 +160,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..f02e779f548 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,23 @@ 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 OMPLPlanProfile&) = delete; + OMPLPlanProfile& operator=(const OMPLPlanProfile&) = delete; + OMPLPlanProfile(OMPLPlanProfile&&) noexcept = delete; + OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = delete; - 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 +95,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/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp index 9002d1b27c0..08231b7275d 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,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -777,4 +779,14 @@ void OMPLDefaultPlanProfile::processOptimizationObjective(OMPLProblem& prob) con } } +template +void OMPLDefaultPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(OMPLPlanProfile); +} + } // 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..45052b5b2b2 100644 --- a/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp +++ b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp @@ -242,7 +242,7 @@ 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; @@ -406,7 +406,7 @@ 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; @@ -501,7 +501,7 @@ 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; diff --git a/tesseract_motion_planners/simple/CMakeLists.txt b/tesseract_motion_planners/simple/CMakeLists.txt index 22d2c2fc3f8..0b630202360 100644 --- a/tesseract_motion_planners/simple/CMakeLists.txt +++ b/tesseract_motion_planners/simple/CMakeLists.txt @@ -2,6 +2,7 @@ add_library( ${PROJECT_NAME}_simple src/interpolation.cpp src/simple_motion_planner.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 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 81ae271d7de..dbef4fa0410 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 83f8ba0907b..341ef430db7 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 5bc86a9a88c..412d550bff8 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 02725e739a5..8fa90c9619d 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 d265b3d9b1d..fe4ff859739 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 @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include namespace tesseract_planning { @@ -42,18 +43,23 @@ 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(); + SimplePlannerPlanProfile(const SimplePlannerPlanProfile&) = delete; + SimplePlannerPlanProfile& operator=(const SimplePlannerPlanProfile&) = delete; + SimplePlannerPlanProfile(SimplePlannerPlanProfile&&) noexcept = delete; + SimplePlannerPlanProfile& operator=(SimplePlannerPlanProfile&&) noexcept = delete; + + /** + * @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 @@ -73,24 +79,40 @@ class SimplePlannerPlanProfile const InstructionPoly& next_instruction, const PlannerRequest& request, 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(); + SimplePlannerCompositeProfile(const SimplePlannerCompositeProfile&) = delete; + SimplePlannerCompositeProfile& operator=(const SimplePlannerCompositeProfile&) = delete; + SimplePlannerCompositeProfile(SimplePlannerCompositeProfile&&) noexcept = delete; + SimplePlannerCompositeProfile& operator=(SimplePlannerCompositeProfile&&) noexcept = delete; + + /** + * @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/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 73836099acd..850db0d7caf 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1348,11 +1348,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 4adc288996b..3356d09a848 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 @@ -36,6 +36,9 @@ #include +#include +#include + namespace tesseract_planning { SimplePlannerFixedSizeAssignPlanProfile::SimplePlannerFixedSizeAssignPlanProfile(int freespace_steps, int linear_steps) @@ -124,4 +127,14 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre return getInterpolatedInstructions(info2.manip->getJointNames(), states, info2.instruction); } +template +void SimplePlannerFixedSizeAssignPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // 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 8857bc7bac2..11c83b703e8 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 + namespace tesseract_planning { SimplePlannerFixedSizePlanProfile::SimplePlannerFixedSizePlanProfile(int freespace_steps, int linear_steps) @@ -63,4 +66,14 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env_state); } +template +void SimplePlannerFixedSizePlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // 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 ccbf1fd781c..be4a21576c4 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 + namespace tesseract_planning { SimplePlannerLVSNoIKPlanProfile::SimplePlannerLVSNoIKPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,14 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru request.env_state); } +template +void SimplePlannerLVSNoIKPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // 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 5ba45d76f26..c38a28264f8 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 + namespace tesseract_planning { SimplePlannerLVSPlanProfile::SimplePlannerLVSPlanProfile(double state_longest_valid_segment_length, @@ -96,4 +99,14 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio request.env_state); } +template +void SimplePlannerLVSPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(SimplePlannerPlanProfile); +} + } // 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/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..a45debb90fb 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 @@ -153,7 +153,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..7da66147c04 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 @@ -98,7 +98,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..908911a1361 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 @@ -60,6 +60,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..f9e332bd107 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 @@ -35,6 +35,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tinyxml2 { @@ -44,18 +45,23 @@ 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(); + TrajOptPlanProfile(const TrajOptPlanProfile&) = delete; + TrajOptPlanProfile& operator=(const TrajOptPlanProfile&) = delete; + TrajOptPlanProfile(TrajOptPlanProfile&&) = delete; + TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = delete; + + /** + * @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 +78,30 @@ 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(); + TrajOptCompositeProfile(const TrajOptCompositeProfile&) = delete; + TrajOptCompositeProfile& operator=(const TrajOptCompositeProfile&) = delete; + TrajOptCompositeProfile(TrajOptCompositeProfile&&) = delete; + TrajOptCompositeProfile& operator=(TrajOptCompositeProfile&&) = delete; + + /** + * @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 +111,45 @@ 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(); + TrajOptSolverProfile(const TrajOptSolverProfile&) = delete; + TrajOptSolverProfile& operator=(const TrajOptSolverProfile&) = delete; + TrajOptSolverProfile(TrajOptSolverProfile&&) = delete; + TrajOptSolverProfile& operator=(TrajOptSolverProfile&&) = delete; + + /** + * @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/src/profile/trajopt_default_composite_profile.cpp b/tesseract_motion_planners/trajopt/src/profile/trajopt_default_composite_profile.cpp index a4d5369e6ae..14f12308bd4 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,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -463,4 +465,14 @@ 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); +} + } // 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..039cc388b84 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,14 @@ 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); +} } // 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..b620a18ce0f 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,14 @@ 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); +} + } // 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/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 446fa1ec105..2b6ad5310cf 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -151,8 +151,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -226,8 +226,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -313,8 +313,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -404,8 +404,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -495,8 +495,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -586,8 +586,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -677,8 +677,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointConstraint) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -742,8 +742,8 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptArrayJointCost) // NOLINT // 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); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); 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..4cb61a3a966 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 @@ -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..55fc37eb31c 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,23 @@ 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(); + TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = delete; + TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = delete; + TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = delete; + TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = delete; + + /** + * @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 +80,30 @@ 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(); + TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = delete; + TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = delete; + TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = delete; + TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = delete; + + /** + * @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 +113,45 @@ 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(); + TrajOptIfoptSolverProfile(const TrajOptIfoptSolverProfile&) = delete; + TrajOptIfoptSolverProfile& operator=(const TrajOptIfoptSolverProfile&) = delete; + TrajOptIfoptSolverProfile(TrajOptIfoptSolverProfile&&) = delete; + TrajOptIfoptSolverProfile& operator=(TrajOptIfoptSolverProfile&&) = delete; + + /** + * @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..fc837f100cb 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,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -97,4 +99,14 @@ 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); +} + } // 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..a2d6994a49a 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 @@ -132,4 +134,14 @@ 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); +} + } // 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..fff3fd303e1 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,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -62,4 +64,14 @@ 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); +} + } // 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_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 3fc1ce07ad7..6fd736e5fba 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -30,7 +30,16 @@ 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/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/min_length_task.cpp b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp index 5fe97069635..32a5a0fa4e6 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -156,11 +156,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/profiles/contact_check_profile.cpp b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp index e7a7c98bedb..1358f2f5015 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -27,6 +27,9 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -36,6 +39,7 @@ 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 +53,17 @@ 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); +} + } // 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..82dc182168a --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp @@ -0,0 +1,49 @@ +/** + * @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); +} + +} // 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..6c5f5e3a9b1 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp @@ -0,0 +1,56 @@ +/** + * @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 + +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); +} + +} // 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..edcf3f7d052 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp @@ -0,0 +1,62 @@ +/** + * @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); +} + +} // 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..eeff731d36b --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -0,0 +1,52 @@ +/** + * @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); +} +} // 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..b70db9798a6 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -0,0 +1,49 @@ +/** + * @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); +} + +} // 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..d6774911474 --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -0,0 +1,82 @@ +/** + * @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); +} + +} // 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..04974fc0a2e --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp @@ -0,0 +1,69 @@ +/** + * @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); +} + +} // 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..10e536b0baf --- /dev/null +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -0,0 +1,55 @@ +/** + * @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); +} + +} // 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..faf33ef9f25 100644 --- a/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp +++ b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp @@ -229,8 +229,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()); @@ -403,8 +402,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()); From eeb1b60a4d5c93a41a96a468c2ea15e31ad51b0e Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Fri, 6 Dec 2024 13:26:53 -0600 Subject: [PATCH 03/13] Add boost serialization to profiles --- .../tesseract_command_language/profile.h | 2 +- .../profile_dictionary.h | 2 +- .../cartesian_waypoint_poly_unit.hpp | 303 ++++++------ .../test_suite/joint_waypoint_poly_unit.hpp | 409 ++++++++-------- .../test_suite/state_waypoint_poly_unit.hpp | 447 +++++++++--------- .../src/profile_dictionary.cpp | 4 +- tesseract_motion_planners/core/CMakeLists.txt | 5 +- .../descartes/CMakeLists.txt | 3 +- .../profile/descartes_default_plan_profile.h | 5 - .../descartes/profile/descartes_profile.h | 4 - .../descartes_default_plan_profile.cpp | 15 + .../ompl/ompl_planner_configurator.h | 93 ++++ .../ompl/profile/ompl_default_plan_profile.h | 5 - .../ompl/profile/ompl_profile.h | 4 - .../ompl/src/ompl_planner_configurator.cpp | 161 +++++++ .../src/profile/ompl_default_plan_profile.cpp | 15 + .../simple/profile/simple_planner_profile.h | 8 - ...planner_fixed_size_assign_plan_profile.cpp | 2 + ...simple_planner_fixed_size_plan_profile.cpp | 2 + .../simple_planner_lvs_no_ik_plan_profile.cpp | 5 + .../simple_planner_lvs_plan_profile.cpp | 5 + .../trajopt_default_composite_profile.h | 5 - .../profile/trajopt_default_plan_profile.h | 5 - .../profile/trajopt_default_solver_profile.h | 7 - .../trajopt/profile/trajopt_profile.h | 12 - .../trajopt/trajopt_collision_config.h | 15 + .../trajopt/trajopt_waypoint_config.h | 15 + .../trajopt_default_composite_profile.cpp | 17 + .../profile/trajopt_default_plan_profile.cpp | 5 + .../trajopt_default_solver_profile.cpp | 5 + .../trajopt/src/trajopt_collision_config.cpp | 29 ++ .../trajopt/src/trajopt_waypoint_config.cpp | 27 ++ .../trajopt_ifopt_default_solver_profile.h | 4 +- .../profile/trajopt_ifopt_profile.h | 12 - ...rajopt_ifopt_default_composite_profile.cpp | 16 + .../trajopt_ifopt_default_plan_profile.cpp | 5 + .../trajopt_ifopt_default_solver_profile.cpp | 5 + .../planning/CMakeLists.txt | 3 +- .../src/profiles/contact_check_profile.cpp | 2 + .../src/profiles/fix_state_bounds_profile.cpp | 4 + .../profiles/fix_state_collision_profile.cpp | 7 + ...rative_spline_parameterization_profile.cpp | 2 + .../src/profiles/min_length_profile.cpp | 1 + .../src/profiles/profile_switch_profile.cpp | 1 + .../ruckig_trajectory_smoothing_profile.cpp | 3 + .../time_optimal_parameterization_profile.cpp | 5 + .../profiles/upsample_trajectory_profile.cpp | 1 + 47 files changed, 1051 insertions(+), 661 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h index 6d73f9de740..2a97d726d2e 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile.h +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -42,7 +42,7 @@ class Profile using ConstPtr = std::shared_ptr; Profile() = default; - Profile(std::size_t id); + Profile(std::size_t key); Profile(const Profile&) = delete; Profile& operator=(const Profile&) = delete; Profile(Profile&&) = delete; 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 d22194faa75..f3e58c4ba5a 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -64,7 +64,7 @@ class ProfileDictionary * @param profile_name The profile name * @param profile The profile to add */ - void addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile); + void addProfile(const std::string& ns, const std::string& profile_name, const Profile::ConstPtr& profile); /** * @brief Check if a profile exists 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/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/src/profile_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index e788657905b..30f3ea26b5d 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -68,7 +68,9 @@ std::unordered_map ProfileDictionary::getProfile ns + "'!"); } -void ProfileDictionary::addProfile(const std::string& ns, const std::string& profile_name, Profile::ConstPtr profile) +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!"); diff --git a/tesseract_motion_planners/core/CMakeLists.txt b/tesseract_motion_planners/core/CMakeLists.txt index 949263a39a2..1ee03a59fe1 100644 --- a/tesseract_motion_planners/core/CMakeLists.txt +++ b/tesseract_motion_planners/core/CMakeLists.txt @@ -1,8 +1,5 @@ # Create interface for core -add_library(${PROJECT_NAME}_core - src/planner.cpp - src/types.cpp - src/utils.cpp) +add_library(${PROJECT_NAME}_core src/planner.cpp src/types.cpp src/utils.cpp) target_link_libraries( ${PROJECT_NAME}_core PUBLIC tesseract::tesseract_environment diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index 097b694dcfb..5eafd22e6a8 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -12,8 +12,7 @@ add_library( src/deserialize.cpp src/descartes_utils.cpp src/profile/descartes_profile.cpp - src/profile/descartes_default_plan_profile.cpp -) + src/profile/descartes_default_plan_profile.cpp) target_link_libraries( ${PROJECT_NAME}_descartes PUBLIC ${PROJECT_NAME}_core 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 a52bdc9ebad..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; 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 b880b75f8d9..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 @@ -54,10 +54,6 @@ class DescartesPlanProfile : public Profile using ConstPtr = std::shared_ptr>; DescartesPlanProfile() : Profile(DescartesPlanProfile::getStaticKey()) {} - DescartesPlanProfile(const DescartesPlanProfile&) = delete; - DescartesPlanProfile& operator=(const DescartesPlanProfile&) = delete; - DescartesPlanProfile(DescartesPlanProfile&&) noexcept = delete; - DescartesPlanProfile& operator=(DescartesPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID 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 f73cc5d87d4..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,8 +24,10 @@ * limitations under the License. */ #include +#include #include #include +#include namespace tesseract_planning { @@ -38,6 +40,19 @@ 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 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 fbb7e9f4c71..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 */ 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 f02e779f548..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 @@ -53,10 +53,6 @@ class OMPLPlanProfile : public Profile using ConstPtr = std::shared_ptr; OMPLPlanProfile(); - OMPLPlanProfile(const OMPLPlanProfile&) = delete; - OMPLPlanProfile& operator=(const OMPLPlanProfile&) = delete; - OMPLPlanProfile(OMPLPlanProfile&&) noexcept = delete; - OMPLPlanProfile& operator=(OMPLPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID 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 08231b7275d..e8a29f36196 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 @@ -35,6 +35,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -52,7 +54,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include #include +#include namespace tesseract_planning { @@ -783,6 +787,17 @@ 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 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 fe4ff859739..05dd83a33ee 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 @@ -50,10 +50,6 @@ class SimplePlannerPlanProfile : public Profile using ConstPtr = std::shared_ptr; SimplePlannerPlanProfile(); - SimplePlannerPlanProfile(const SimplePlannerPlanProfile&) = delete; - SimplePlannerPlanProfile& operator=(const SimplePlannerPlanProfile&) = delete; - SimplePlannerPlanProfile(SimplePlannerPlanProfile&&) noexcept = delete; - SimplePlannerPlanProfile& operator=(SimplePlannerPlanProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID @@ -93,10 +89,6 @@ class SimplePlannerCompositeProfile : public Profile using ConstPtr = std::shared_ptr; SimplePlannerCompositeProfile(); - SimplePlannerCompositeProfile(const SimplePlannerCompositeProfile&) = delete; - SimplePlannerCompositeProfile& operator=(const SimplePlannerCompositeProfile&) = delete; - SimplePlannerCompositeProfile(SimplePlannerCompositeProfile&&) noexcept = delete; - SimplePlannerCompositeProfile& operator=(SimplePlannerCompositeProfile&&) noexcept = delete; /** * @brief A utility function for getting profile ID 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 3356d09a848..9bf5c2e4751 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 @@ -131,6 +131,8 @@ 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 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 11c83b703e8..b6efdedb9bb 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 @@ -70,6 +70,8 @@ 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 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 be4a21576c4..5efa23ecb44 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 @@ -103,6 +103,11 @@ 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 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 c38a28264f8..fc54a00112f 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 @@ -103,6 +103,11 @@ 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 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 a45debb90fb..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 }; 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 7da66147c04..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; 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 908911a1361..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 }; 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 f9e332bd107..a5323148c90 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 @@ -52,10 +52,6 @@ class TrajOptPlanProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptPlanProfile(); - TrajOptPlanProfile(const TrajOptPlanProfile&) = delete; - TrajOptPlanProfile& operator=(const TrajOptPlanProfile&) = delete; - TrajOptPlanProfile(TrajOptPlanProfile&&) = delete; - TrajOptPlanProfile& operator=(TrajOptPlanProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -92,10 +88,6 @@ class TrajOptCompositeProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptCompositeProfile(); - TrajOptCompositeProfile(const TrajOptCompositeProfile&) = delete; - TrajOptCompositeProfile& operator=(const TrajOptCompositeProfile&) = delete; - TrajOptCompositeProfile(TrajOptCompositeProfile&&) = delete; - TrajOptCompositeProfile& operator=(TrajOptCompositeProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -125,10 +117,6 @@ class TrajOptSolverProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptSolverProfile(); - TrajOptSolverProfile(const TrajOptSolverProfile&) = delete; - TrajOptSolverProfile& operator=(const TrajOptSolverProfile&) = delete; - TrajOptSolverProfile(TrajOptSolverProfile&&) = delete; - TrajOptSolverProfile& operator=(TrajOptSolverProfile&&) = delete; /** * @brief A utility function for getting profile ID 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 14f12308bd4..8ee1838e8d5 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 @@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -43,6 +44,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01; @@ -469,6 +471,21 @@ 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 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 039cc388b84..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 @@ -318,6 +318,11 @@ 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 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 b620a18ce0f..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 @@ -50,6 +50,11 @@ 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 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_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_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 4cb61a3a966..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 */ 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 55fc37eb31c..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 @@ -54,10 +54,6 @@ class TrajOptIfoptPlanProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptPlanProfile(); - TrajOptIfoptPlanProfile(const TrajOptIfoptPlanProfile&) = delete; - TrajOptIfoptPlanProfile& operator=(const TrajOptIfoptPlanProfile&) = delete; - TrajOptIfoptPlanProfile(TrajOptIfoptPlanProfile&&) = delete; - TrajOptIfoptPlanProfile& operator=(TrajOptIfoptPlanProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -94,10 +90,6 @@ class TrajOptIfoptCompositeProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptCompositeProfile(); - TrajOptIfoptCompositeProfile(const TrajOptIfoptCompositeProfile&) = delete; - TrajOptIfoptCompositeProfile& operator=(const TrajOptIfoptCompositeProfile&) = delete; - TrajOptIfoptCompositeProfile(TrajOptIfoptCompositeProfile&&) = delete; - TrajOptIfoptCompositeProfile& operator=(TrajOptIfoptCompositeProfile&&) = delete; /** * @brief A utility function for getting profile ID @@ -127,10 +119,6 @@ class TrajOptIfoptSolverProfile : public Profile using ConstPtr = std::shared_ptr; TrajOptIfoptSolverProfile(); - TrajOptIfoptSolverProfile(const TrajOptIfoptSolverProfile&) = delete; - TrajOptIfoptSolverProfile& operator=(const TrajOptIfoptSolverProfile&) = delete; - TrajOptIfoptSolverProfile(TrajOptIfoptSolverProfile&&) = delete; - TrajOptIfoptSolverProfile& operator=(TrajOptIfoptSolverProfile&&) = delete; /** * @brief A utility function for getting profile ID 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 fc837f100cb..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,8 +29,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -38,6 +40,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include +#include namespace tesseract_planning { @@ -103,6 +107,18 @@ 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 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 a2d6994a49a..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 @@ -43,6 +43,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include + namespace tesseract_planning { void TrajOptIfoptDefaultPlanProfile::apply(TrajOptIfoptProblem& problem, @@ -138,6 +140,9 @@ 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 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 fff3fd303e1..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 @@ -30,6 +30,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -68,6 +69,10 @@ 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 diff --git a/tesseract_task_composer/planning/CMakeLists.txt b/tesseract_task_composer/planning/CMakeLists.txt index 6fd736e5fba..d42dc32dd0d 100644 --- a/tesseract_task_composer/planning/CMakeLists.txt +++ b/tesseract_task_composer/planning/CMakeLists.txt @@ -38,8 +38,7 @@ set(LIB_SOURCE_FILES 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 - ) + src/profiles/upsample_trajectory_profile.cpp) set(LIB_SOURCE_LINK_LIBRARIES ${PROJECT_NAME} 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 1358f2f5015..5288c3e0d30 100644 --- a/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/contact_check_profile.cpp @@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include namespace tesseract_planning { @@ -60,6 +61,7 @@ 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 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 index 82dc182168a..20d830f90a5 100644 --- a/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/fix_state_bounds_profile.cpp @@ -40,6 +40,10 @@ 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 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 index 6c5f5e3a9b1..3ee61e36348 100644 --- a/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/fix_state_collision_profile.cpp @@ -25,8 +25,10 @@ */ #include +#include #include #include +#include #include namespace tesseract_planning @@ -47,6 +49,11 @@ 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 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 index edcf3f7d052..15dfe2a2db1 100644 --- a/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/iterative_spline_parameterization_profile.cpp @@ -53,6 +53,8 @@ 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 diff --git a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp index eeff731d36b..8036add1450 100644 --- a/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/min_length_profile.cpp @@ -44,6 +44,7 @@ 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 diff --git a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp index b70db9798a6..f6ce5d1e071 100644 --- a/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/profile_switch_profile.cpp @@ -40,6 +40,7 @@ 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 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 index d6774911474..12113f0cf6f 100644 --- a/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -73,6 +73,9 @@ 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 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 index 04974fc0a2e..63b2c838f35 100644 --- a/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/time_optimal_parameterization_profile.cpp @@ -60,6 +60,11 @@ 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 diff --git a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp index 10e536b0baf..514400a7636 100644 --- a/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/upsample_trajectory_profile.cpp @@ -46,6 +46,7 @@ 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 From 8729b17972fc437ddbca705937b480f05c8f0916 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 7 Dec 2024 11:04:58 -0600 Subject: [PATCH 04/13] Modify the concept of profile overrides in Composite and Move Instruction --- .../composite_instruction.h | 10 +- .../move_instruction.h | 17 +- .../poly/move_instruction_poly.h | 51 +- .../profile_dictionary.h | 12 +- .../test_suite/move_instruction_poly_unit.hpp | 75 +- .../tesseract_command_language/types.h | 45 + .../src/composite_instruction.cpp | 15 +- .../src/move_instruction.cpp | 28 +- .../src/poly/move_instruction_poly.cpp | 21 +- .../src/profile_dictionary.cpp | 56 + .../test/command_language_unit.cpp | 16 +- .../tesseract_motion_planners/core/types.h | 21 - .../tesseract_motion_planners/planner_utils.h | 57 - .../core/test/CMakeLists.txt | 24 - .../core/test/utils_test.cpp | 1715 ----------------- .../impl/descartes_motion_planner.hpp | 12 +- .../examples/chain_example.cpp | 9 +- .../examples/freespace_example.cpp | 9 +- .../examples/raster_example.cpp | 37 +- .../ompl/src/ompl_motion_planner.cpp | 8 +- .../src/profile/ompl_default_plan_profile.cpp | 1 - .../simple/src/simple_motion_planner.cpp | 17 +- .../trajopt/src/trajopt_motion_planner.cpp | 26 +- .../trajopt/test/trajopt_planner_tests.cpp | 17 + .../src/trajopt_ifopt_motion_planner.cpp | 31 +- .../config/task_composer_plugins.yaml | 88 - .../examples/task_composer_raster_example.cpp | 1 + .../task_composer_trajopt_example.cpp | 1 + .../nodes/continuous_contact_check_task.h | 1 - .../nodes/discrete_contact_check_task.h | 1 - .../planning/nodes/fix_state_bounds_task.h | 1 - .../planning/nodes/fix_state_collision_task.h | 1 - .../iterative_spline_parameterization_task.h | 2 - .../planning/nodes/min_length_task.h | 3 - .../planning/nodes/motion_planner_task.hpp | 18 - .../planning/nodes/profile_switch_task.h | 3 - .../nodes/ruckig_trajectory_smoothing_task.h | 2 - .../time_optimal_parameterization_task.h | 2 - .../planning/nodes/upsample_trajectory_task.h | 3 - .../nodes/continuous_contact_check_task.cpp | 8 +- .../src/nodes/discrete_contact_check_task.cpp | 8 +- .../src/nodes/fix_state_bounds_task.cpp | 8 +- .../src/nodes/fix_state_collision_task.cpp | 10 +- ...iterative_spline_parameterization_task.cpp | 18 +- .../planning/src/nodes/min_length_task.cpp | 11 +- .../src/nodes/profile_switch_task.cpp | 10 +- .../ruckig_trajectory_smoothing_task.cpp | 16 +- .../time_optimal_parameterization_task.cpp | 11 +- .../src/nodes/upsample_trajectory_task.cpp | 12 +- .../tesseract_task_composer_planning_unit.cpp | 68 +- 50 files changed, 354 insertions(+), 2283 deletions(-) create mode 100644 tesseract_command_language/include/tesseract_command_language/types.h delete mode 100644 tesseract_motion_planners/core/test/utils_test.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/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_dictionary.h b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h index f3e58c4ba5a..6c7548fcb2f 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h +++ b/tesseract_command_language/include/tesseract_command_language/profile_dictionary.h @@ -38,15 +38,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -/** - * This used to store specific profile mapping with the request - * - * For example say you have a profile named Raster in your command language. Say you have multiple Raster profiles - * for descartes {Raster, Raster1, Raster2}. This allows you to remap the meaning of Raster in the command language to - * say Raster2 for the specific planner Descartes by Map>. - */ -using ProfileRemapping = std::unordered_map>; - /** * @brief This class is used to store profiles used by various tasks * @details This is a thread safe class @@ -65,6 +56,9 @@ class ProfileDictionary * @param profile The profile to add */ 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 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/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_dictionary.cpp b/tesseract_command_language/src/profile_dictionary.cpp index 30f3ea26b5d..c27f9660acb 100644 --- a/tesseract_command_language/src/profile_dictionary.cpp +++ b/tesseract_command_language/src/profile_dictionary.cpp @@ -105,6 +105,61 @@ void ProfileDictionary::addProfile(const std::string& ns, } } +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_); @@ -151,6 +206,7 @@ void ProfileDictionary::clear() template void ProfileDictionary::serialize(Archive& ar, const unsigned int /*version*/) { + const std::shared_lock lock(mutex_); ar& boost::serialization::make_nvp("profiles", profiles_); } 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_motion_planners/core/include/tesseract_motion_planners/core/types.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h index e2e9f0910ca..2d8d816ea2c 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 @@ -77,18 +68,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 9d0eb86da14..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 @@ -159,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(ProfileType::getStaticKey(), ns, profile)) - return std::static_pointer_cast(overrides->getProfile(ProfileType::getStaticKey(), 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/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/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..105fc082f52 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 @@ -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/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/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 7bc73244994..2532d3fdd4d 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"); 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 e8a29f36196..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 @@ -56,7 +56,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include namespace tesseract_planning { diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index bb711147f55..81c3ff99346 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -215,18 +215,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/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 2cd3e796e1a..022c3de54da 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"); @@ -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/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 2b6ad5310cf..869b3341d74 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 @@ -148,11 +149,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsJointJoint) // N // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -223,11 +226,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -310,11 +315,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceJointCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -401,11 +408,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartJoint) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -492,11 +501,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptFreespaceCartCart) // NOLINT // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -583,11 +594,13 @@ TEST_F(TesseractPlanningTrajoptUnit, TrajoptPlannerBooleanFlagsCartCart) // NOL // Create Profiles auto plan_profile = std::make_shared(); auto composite_profile = std::make_shared(); + auto solver_profile = std::make_shared(); // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile); + profiles->addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -674,11 +687,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", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); @@ -739,11 +754,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", solver_profile); // Create Planner TrajOptMotionPlanner test_planner(TRAJOPT_DEFAULT_NAMESPACE); 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..afc5658b3c1 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 @@ -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"); @@ -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/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/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..b4e30e60910 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); @@ -192,10 +185,6 @@ class MotionPlannerTask : public TaskComposerTask 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 +238,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/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 32a5a0fa4e6..352e6c86bd3 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) { 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/test/tesseract_task_composer_planning_unit.cpp b/tesseract_task_composer/test/tesseract_task_composer_planning_unit.cpp index faf33ef9f25..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); @@ -272,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); @@ -1111,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); @@ -1286,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); @@ -1437,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); @@ -1880,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); @@ -2007,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)"; @@ -2016,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); @@ -2200,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); @@ -2394,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); @@ -2595,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)"; @@ -2604,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); From 193db1289ebe5f049758856fe2f668614fe8f660 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 8 Dec 2024 20:47:32 -0600 Subject: [PATCH 05/13] Address some windows build issues (#541) --- .github/workflows/mac.yml | 2 +- ...ask_composer_plugins_no_trajopt_ifopt.yaml | 370 ++++++++++++++---- .../core/task_composer_node.h | 27 +- .../core/task_composer_node_info.h | 6 +- .../core/task_composer_node_types.h | 40 ++ .../core/task_composer_plugin_factory.h | 4 +- .../core/src/task_composer_node.cpp | 29 +- .../core/src/task_composer_node_info.cpp | 3 + 8 files changed, 364 insertions(+), 117 deletions(-) create mode 100644 tesseract_task_composer/core/include/tesseract_task_composer/core/task_composer_node_types.h 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/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()); From ad7330935f538bf5501672a8054180bafeab7d75 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 10 Dec 2024 20:13:42 -0600 Subject: [PATCH 06/13] Fix windows build (#542) Co-authored-by: John Wason --- .github/workflows/windows.yml | 2 +- .../tesseract_motion_planners/trajopt/profile/trajopt_profile.h | 1 + .../trajopt/src/profile/trajopt_default_composite_profile.cpp | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) 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_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 a5323148c90..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,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include 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 8ee1838e8d5..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 @@ -45,6 +45,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include static const double LONGEST_VALID_SEGMENT_FRACTION_DEFAULT = 0.01; From d6c4f3faccc35ff60fbc8a740eaf03e2b792795a Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 14 Dec 2024 13:33:31 -0600 Subject: [PATCH 07/13] Remove env_state from planning request --- .../tesseract_motion_planners/core/types.h | 3 - .../impl/descartes_motion_planner.hpp | 2 +- .../test/descartes_planner_tests.cpp | 32 +--------- .../ompl/src/ompl_motion_planner.cpp | 4 +- .../ompl/test/ompl_planner_tests.cpp | 16 ++--- .../simple/interpolation.h | 1 - .../simple/simple_motion_planner.h | 6 +- .../simple/src/interpolation.cpp | 2 - ...planner_fixed_size_assign_plan_profile.cpp | 8 +-- ...simple_planner_fixed_size_plan_profile.cpp | 8 +-- .../simple_planner_lvs_no_ik_plan_profile.cpp | 8 +-- .../simple_planner_lvs_plan_profile.cpp | 8 +-- .../simple/src/simple_motion_planner.cpp | 25 ++++---- ...ple_planner_fixed_size_assign_position.cpp | 14 ++--- ...imple_planner_fixed_size_interpolation.cpp | 16 ++--- .../test/simple_planner_lvs_interpolation.cpp | 27 ++++----- .../trajopt/src/trajopt_motion_planner.cpp | 2 +- .../trajopt/test/trajopt_planner_tests.cpp | 59 +++---------------- .../src/trajopt_ifopt_motion_planner.cpp | 4 +- .../planning/nodes/motion_planner_task.hpp | 1 - .../planning/src/nodes/min_length_task.cpp | 1 - 21 files changed, 82 insertions(+), 165 deletions(-) diff --git a/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h b/tesseract_motion_planners/core/include/tesseract_motion_planners/core/types.h index 2d8d816ea2c..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 @@ -56,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; 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 105fc082f52..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(); diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index df74d6dccd9..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,8 +130,7 @@ 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(); @@ -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(); @@ -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(); @@ -370,7 +345,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) 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/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index 2532d3fdd4d..c35aaa89b11 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -345,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/test/ompl_planner_tests.cpp b/tesseract_motion_planners/ompl/test/ompl_planner_tests.cpp index 45052b5b2b2..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(); @@ -248,7 +247,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespacePlannerUnit) // NOLINT 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(); @@ -412,7 +409,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianGoalPlannerUnit) // NOLINT 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(); @@ -507,7 +502,6 @@ TYPED_TEST(OMPLTestFixture, OMPLFreespaceCartesianStartPlannerUnit) // NOLINT 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/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h index 706d1d9a63a..9d7e02cfb88 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 @@ -515,7 +515,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/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 850db0d7caf..5f8e66cbf42 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -1325,7 +1325,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, @@ -1335,7 +1334,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 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 9bf5c2e4751..66523dc6c9c 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 @@ -33,7 +33,7 @@ #include #include - +#include #include #include @@ -54,8 +54,8 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); Eigen::MatrixXd states; if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) @@ -90,7 +90,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre } else { - Eigen::VectorXd seed = request.env_state.getJointValues(info2.manip->getJointNames()); + Eigen::VectorXd seed = request.env->getCurrentJointValues(info2.manip->getJointNames()); tesseract_common::enforceLimits(seed, info2.manip->getLimits().joint_limits); if (info2.instruction.isLinear()) 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 b6efdedb9bb..564d41642e9 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 @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -51,8 +51,8 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps); @@ -63,7 +63,7 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps); - return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env_state); + return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env->getState()); } template 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 5efa23ecb44..ae925a3aefc 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 @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -59,8 +59,8 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - JointGroupInstructionInfo info2(base_instruction, request, global_manip_info); + JointGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + JointGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru rotation_longest_valid_segment_length, min_steps, max_steps, - request.env_state); + request.env->getState()); } template 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 fc54a00112f..b17f2d72dc6 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 @@ -30,7 +30,7 @@ #include #include - +#include #include #include @@ -59,8 +59,8 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio const PlannerRequest& request, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, request, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, request, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio rotation_longest_valid_segment_length, min_steps, max_steps, - request.env_state); + request.env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index 81c3ff99346..05ce62916ee 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; @@ -186,7 +189,7 @@ CompositeInstruction SimpleMotionPlanner::processCompositeInstruction(const Comp { // Run IK to find solution closest to start KinematicGroupInstructionInfo info(prev_instruction, request, 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)); } diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 85f4d6301d1..4f0f74ff258 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -72,11 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -114,11 +114,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -155,11 +155,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -170,7 +170,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte std::vector move_instructions = profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); - Eigen::VectorXd position = request.env_state.getJointValues(fwd_kin->getJointNames()); + Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp index 2ff99cffb92..fe8d2218d04 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp @@ -72,11 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -114,11 +114,11 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp2.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); @@ -160,12 +160,12 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -203,12 +203,12 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp2.getTransform().translation() = Eigen::Vector3d(0.25, 0.1, 1); diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp index 49b834f42f6..584a466f588 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp @@ -72,12 +72,11 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -129,13 +128,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -197,14 +196,14 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -255,13 +254,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); + auto joint_group = request.env->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -324,7 +323,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -332,7 +330,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -381,7 +379,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -389,7 +386,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); JointWaypointPoly wp2{ JointWaypoint(joint_names_, Eigen::VectorXd::Ones(7)) }; MoveInstruction instr2(wp2, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -450,7 +447,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -458,7 +454,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; @@ -509,7 +505,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo { PlannerRequest request; request.env = env_; - request.env_state = env_->getState(); auto joint_group = env_->getJointGroup(manip_info_.manipulator); @@ -517,7 +512,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo joint_group->calcFwdKin(Eigen::VectorXd::Zero(7)).at(manip_info_.tcp_frame)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; - instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, request.env_state.getJointValues(joint_names_))); + instr1_seed.assignJointWaypoint(JointWaypoint(joint_names_, env_->getCurrentJointValues(joint_names_))); CartesianWaypointPoly wp2{ CartesianWaypoint( joint_group->calcFwdKin(Eigen::VectorXd::Ones(7)).at(manip_info_.tcp_frame)) }; diff --git a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp index 022c3de54da..4959dac7b79 100644 --- a/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp +++ b/tesseract_motion_planners/trajopt/src/trajopt_motion_planner.cpp @@ -282,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 */ diff --git a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp index 869b3341d74..75bd1867e35 100644 --- a/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp +++ b/tesseract_motion_planners/trajopt/test/trajopt_planner_tests.cpp @@ -120,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)) }; @@ -143,8 +142,7 @@ 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(); @@ -164,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 @@ -197,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)) }; @@ -220,8 +216,7 @@ 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(); @@ -241,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; { @@ -286,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)) }; @@ -309,8 +302,7 @@ 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(); @@ -330,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; { @@ -373,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) * @@ -402,8 +389,7 @@ 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(); @@ -423,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; { @@ -466,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)) }; @@ -495,8 +474,7 @@ 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(); @@ -516,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; { @@ -559,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)) }; @@ -588,8 +559,7 @@ 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(); @@ -609,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; @@ -655,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"); @@ -677,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(); @@ -702,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); @@ -721,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"); @@ -743,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(); @@ -769,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/src/trajopt_ifopt_motion_planner.cpp b/tesseract_motion_planners/trajopt_ifopt/src/trajopt_ifopt_motion_planner.cpp index afc5658b3c1..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 @@ -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 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 b4e30e60910..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 @@ -181,7 +181,6 @@ class MotionPlannerTask : public TaskComposerTask // Fill out request // -------------------- PlannerRequest request; - request.env_state = env->getState(); request.env = env; request.instructions = instructions; request.profiles = profiles; 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 352e6c86bd3..1c090e2d1a9 100644 --- a/tesseract_task_composer/planning/src/nodes/min_length_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/min_length_task.cpp @@ -137,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 From 12faec533295d45bd8ffd4dfbaf532dd7736cb32 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 14 Dec 2024 13:48:50 -0600 Subject: [PATCH 08/13] Remove PlanningRequest form simple profile interface --- .../simple/interpolation.h | 8 -- ...e_planner_fixed_size_assign_plan_profile.h | 2 +- .../simple_planner_fixed_size_plan_profile.h | 2 +- .../simple_planner_lvs_no_ik_plan_profile.h | 2 +- .../profile/simple_planner_lvs_plan_profile.h | 2 +- .../simple/profile/simple_planner_profile.h | 3 +- .../simple/src/interpolation.cpp | 14 ---- ...planner_fixed_size_assign_plan_profile.cpp | 8 +- ...simple_planner_fixed_size_plan_profile.cpp | 8 +- .../simple_planner_lvs_no_ik_plan_profile.cpp | 8 +- .../simple_planner_lvs_plan_profile.cpp | 8 +- .../simple/src/simple_motion_planner.cpp | 5 +- ...ple_planner_fixed_size_assign_position.cpp | 15 +--- ...imple_planner_fixed_size_interpolation.cpp | 20 +---- .../test/simple_planner_lvs_interpolation.cpp | 82 +++++++------------ 15 files changed, 61 insertions(+), 126 deletions(-) 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 9d7e02cfb88..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 @@ -46,10 +46,6 @@ namespace tesseract_planning /** @brief The Joint Group Instruction Information struct */ struct JointGroupInstructionInfo { - JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info); - JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info); @@ -101,10 +97,6 @@ struct JointGroupInstructionInfo /** @brief The Kinematic Group Instruction Information struct */ struct KinematicGroupInstructionInfo { - KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info); - KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info); 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 dbef4fa0410..a652400a496 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 @@ -48,7 +48,7 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The number of steps to use for freespace instruction */ 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 341ef430db7..85cb50a59db 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 @@ -47,7 +47,7 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The number of steps to use for freespace instruction */ 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 412d550bff8..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 @@ -61,7 +61,7 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ 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 8fa90c9619d..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 @@ -60,7 +60,7 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const override; /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */ 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 05dd83a33ee..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 @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include @@ -73,7 +74,7 @@ class SimplePlannerPlanProfile : public Profile const MoveInstructionPoly& prev_seed, const MoveInstructionPoly& base_instruction, const InstructionPoly& next_instruction, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const = 0; protected: diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp index 5f8e66cbf42..7766a23d7fd 100644 --- a/tesseract_motion_planners/simple/src/interpolation.cpp +++ b/tesseract_motion_planners/simple/src/interpolation.cpp @@ -55,13 +55,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_planning { -JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info) - : JointGroupInstructionInfo(plan_instruction, *request.env, manip_info) -{ -} - JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info) @@ -129,13 +122,6 @@ const Eigen::VectorXd& JointGroupInstructionInfo::extractJointPosition() const return getJointPosition(instruction.getWaypoint()); } -KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, - const PlannerRequest& request, - const tesseract_common::ManipulatorInfo& manip_info) - : KinematicGroupInstructionInfo(plan_instruction, *request.env, manip_info) -{ -} - KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction, const tesseract_environment::Environment& env, const tesseract_common::ManipulatorInfo& manip_info) 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 66523dc6c9c..6fcd5097e78 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 @@ -51,11 +51,11 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); Eigen::MatrixXd states; if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) @@ -90,7 +90,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre } else { - Eigen::VectorXd seed = request.env->getCurrentJointValues(info2.manip->getJointNames()); + Eigen::VectorXd seed = env->getCurrentJointValues(info2.manip->getJointNames()); tesseract_common::enforceLimits(seed, info2.manip->getLimits().joint_limits); if (info2.instruction.isLinear()) 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 564d41642e9..47285a69f75 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 @@ -48,11 +48,11 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps); @@ -63,7 +63,7 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps); - return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env->getState()); + return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState()); } template 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 ae925a3aefc..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 @@ -56,11 +56,11 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - JointGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - JointGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + JointGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + JointGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru rotation_longest_valid_segment_length, min_steps, max_steps, - request.env->getState()); + env->getState()); } template 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 b17f2d72dc6..f74318b9354 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 @@ -56,11 +56,11 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio const MoveInstructionPoly& /*prev_seed*/, const MoveInstructionPoly& base_instruction, const InstructionPoly& /*next_instruction*/, - const PlannerRequest& request, + const std::shared_ptr& env, const tesseract_common::ManipulatorInfo& global_manip_info) const { - KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info); - KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info); + KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info); + KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info); if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint) return interpolateJointJointWaypoint(info1, @@ -96,7 +96,7 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio rotation_longest_valid_segment_length, min_steps, max_steps, - request.env->getState()); + env->getState()); } template diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp index 05ce62916ee..bdb8356a8ec 100644 --- a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp +++ b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp @@ -188,7 +188,8 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr if (!start_waypoint.as().hasSeed()) { // Run IK to find solution closest to start - KinematicGroupInstructionInfo info(prev_instruction, request, request.instructions.getManipulatorInfo()); + KinematicGroupInstructionInfo info( + prev_instruction, *request.env, request.instructions.getManipulatorInfo()); auto start_seed = getClosestJointSolution(info, start_state.getJointValues(manip->getJointNames())); start_waypoint.as().setSeed( tesseract_common::JointState(manip->getJointNames(), start_seed)); @@ -239,7 +240,7 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr prev_seed, base_instruction, next_instruction, - request, + request.env, request.instructions.getManipulatorInfo()); // The data for the last instruction should be unchanged with exception to seed or tolerance joint state diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp index 4f0f74ff258..53731428630 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testi TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -112,9 +109,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -127,7 +121,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -153,9 +147,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCartesian_AssignJointPosition) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -168,7 +159,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte SimplePlannerFixedSizeAssignPlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); auto fwd_kin = env_->getJointGroup(manip_info_.manipulator); Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames()); EXPECT_EQ(move_instructions.size(), 10); diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp index fe8d2218d04..4e8fd894023 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_interpolation.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerFixedSizeInterpolationUnit : public ::testin TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -112,9 +109,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointJoint_Join TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -128,7 +122,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -158,9 +152,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, JointCart_Joint TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, 0, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -174,7 +165,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { @@ -201,9 +192,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartJoint_Joint TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointInterpolation) // NOLINT { - PlannerRequest request; - request.env = env_; - CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) }; wp1.getTransform().translation() = Eigen::Vector3d(0.25, -0.1, 1); MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); @@ -218,7 +206,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeInterpolationUnit, CartCart_JointI SimplePlannerFixedSizePlanProfile profile(10, 10); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(move_instructions.size(), 10); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { diff --git a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp index 584a466f588..f7cc4431d98 100644 --- a/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp +++ b/tesseract_motion_planners/simple/test/simple_planner_lvs_interpolation.cpp @@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerLVSInterpolationUnit : public ::testing::Tes TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_); MoveInstruction instr1_seed{ instr1 }; @@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); auto move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -111,13 +108,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.05; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double dist = (wp1.getPosition() - wp2.getPosition()).norm(); int steps = int(dist / longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(cl.size()) > min_steps); @@ -126,9 +123,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointJoint_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; @@ -143,7 +137,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -168,13 +162,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used when large motion given double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p1 = joint_group->calcFwdKin(wp1.getPosition()).at(manip_info_.tcp_frame); Eigen::Isometry3d p2 = joint_group->calcFwdKin(wp2.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (p2.translation() - p1.translation()).norm(); @@ -185,7 +179,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(p2.linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -194,9 +188,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; @@ -213,7 +204,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -240,22 +231,19 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_JointCart_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - - auto joint_group = request.env->getJointGroup(manip_info_.manipulator); + auto joint_group = env_->getJointGroup(manip_info_.manipulator); JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) }; MoveInstruction instr1(wp1, MoveInstructionType::LINEAR, "TEST_PROFILE", manip_info_); @@ -270,7 +258,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -296,13 +284,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p1 = joint_group->calcFwdKin(wp1.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (wp2.getTransform().translation() - p1.translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; @@ -312,7 +300,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(p1.linear()).angularDistance(Eigen::Quaterniond(wp2.getTransform().linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -321,9 +309,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -339,7 +324,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -365,21 +350,18 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartJoint_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -395,7 +377,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -420,13 +402,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); Eigen::Isometry3d p2 = joint_group->calcFwdKin(wp2.getPosition()).at(manip_info_.tcp_frame); double trans_dist = (p2.translation() - wp1.getTransform().translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; @@ -436,7 +418,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(wp1.getTransform().linear()).angularDistance(Eigen::Quaterniond(p2.linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(crl.size()) > min_steps); @@ -445,9 +427,6 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Freespace) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -464,7 +443,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -491,21 +470,18 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 0.5, 1.57, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure state_longest_valid_segment_length is used double longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile cl_profile(longest_valid_segment_length, 10, 6.28, min_steps); - auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cl = cl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_TRUE(static_cast(cl.size()) > min_steps); } TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypoint_CartCart_Linear) // NOLINT { - PlannerRequest request; - request.env = env_; - auto joint_group = env_->getJointGroup(manip_info_.manipulator); CartesianWaypointPoly wp1{ CartesianWaypoint( @@ -522,7 +498,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo SimplePlannerLVSPlanProfile profile(3.14, 0.5, 1.57, 5); std::vector move_instructions = - profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); for (std::size_t i = 0; i < move_instructions.size() - 1; ++i) { const MoveInstructionPoly& mi = move_instructions.at(i); @@ -548,13 +524,13 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure equal to minimum number steps when all params set large int min_steps = 5; SimplePlannerLVSPlanProfile cs_profile(6.28, 10, 6.28, min_steps); - auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto cs = cs_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); EXPECT_EQ(cs.size(), min_steps); // Ensure translation_longest_valid_segment_length is used double translation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile ctl_profile(6.28, translation_longest_valid_segment_length, 6.28, min_steps); - auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto ctl = ctl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double trans_dist = (wp2.getTransform().translation() - wp1.getTransform().translation()).norm(); int trans_steps = int(trans_dist / translation_longest_valid_segment_length) + 1; EXPECT_TRUE(static_cast(ctl.size()) > min_steps); @@ -563,7 +539,7 @@ TEST_F(TesseractPlanningSimplePlannerLVSInterpolationUnit, InterpolateStateWaypo // Ensure rotation_longest_valid_segment_length is used double rotation_longest_valid_segment_length = 0.01; SimplePlannerLVSPlanProfile crl_profile(6.28, 10, rotation_longest_valid_segment_length, min_steps); - auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo()); + auto crl = crl_profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo()); double rot_dist = Eigen::Quaterniond(wp1.getTransform().linear()).angularDistance(Eigen::Quaterniond(wp2.getTransform().linear())); int rot_steps = int(rot_dist / rotation_longest_valid_segment_length) + 1; From 9e90254a654562b43b12892bcf6a83e183bf4cc3 Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Mon, 16 Dec 2024 11:21:12 -0600 Subject: [PATCH 09/13] Added issue templates (#549) --- .github/ISSUE_TEMPLATE/bug-report.yml | 57 ++++++++++++++++++++++ .github/ISSUE_TEMPLATE/feature-request.yml | 32 ++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 .github/ISSUE_TEMPLATE/bug-report.yml create mode 100644 .github/ISSUE_TEMPLATE/feature-request.yml 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. From b5d897fb61620db3b57de31543059eaf08368098 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 18 Dec 2024 13:06:55 +0100 Subject: [PATCH 10/13] Add Ruckig composite profile serialization --- .../profiles/ruckig_trajectory_smoothing_profile.h | 6 ++++++ .../ruckig_trajectory_smoothing_profile.cpp | 13 +++++++++++++ 2 files changed, 19 insertions(+) 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 8b99bce65b3..40132a64ab7 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 @@ -64,6 +64,11 @@ struct RuckigTrajectorySmoothingCompositeProfile : public Profile /** @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 }; struct RuckigTrajectorySmoothingMoveProfile : public Profile @@ -96,6 +101,7 @@ struct RuckigTrajectorySmoothingMoveProfile : public Profile }; } // namespace tesseract_planning +BOOST_CLASS_EXPORT_KEY(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) BOOST_CLASS_EXPORT_KEY(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) #endif // TESSERACT_TASK_COMPOSER_RUCKIG_TRAJECTORY_SMOOTHING_PROFILE_H 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 index 12113f0cf6f..471fcff5346 100644 --- a/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp +++ b/tesseract_task_composer/planning/src/profiles/ruckig_trajectory_smoothing_profile.cpp @@ -52,6 +52,17 @@ std::size_t RuckigTrajectorySmoothingCompositeProfile::getStaticKey() return std::type_index(typeid(RuckigTrajectorySmoothingCompositeProfile)).hash_code(); } +template +void RuckigTrajectorySmoothingCompositeProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); + ar& BOOST_SERIALIZATION_NVP(duration_extension_fraction); + ar& BOOST_SERIALIZATION_NVP(max_duration_extension_factor); + ar& BOOST_SERIALIZATION_NVP(max_velocity_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_acceleration_scaling_factor); + ar& BOOST_SERIALIZATION_NVP(max_jerk_scaling_factor); +} + RuckigTrajectorySmoothingMoveProfile::RuckigTrajectorySmoothingMoveProfile() : Profile(RuckigTrajectorySmoothingMoveProfile::getStaticKey()) { @@ -81,5 +92,7 @@ void RuckigTrajectorySmoothingMoveProfile::serialize(Archive& ar, const unsigned } // namespace tesseract_planning #include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) From 51f87af5e1d30a195d03e5d8b680feb78592d95d Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 18 Dec 2024 15:58:58 +0100 Subject: [PATCH 11/13] Do not delete special member functions (#551) --- .../include/tesseract_command_language/profile.h | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/tesseract_command_language/include/tesseract_command_language/profile.h b/tesseract_command_language/include/tesseract_command_language/profile.h index 2a97d726d2e..205a76b8148 100644 --- a/tesseract_command_language/include/tesseract_command_language/profile.h +++ b/tesseract_command_language/include/tesseract_command_language/profile.h @@ -43,10 +43,6 @@ class Profile 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; /** @@ -56,6 +52,11 @@ class Profile std::size_t getKey() const; protected: + Profile(const Profile&) = default; + Profile& operator=(const Profile&) = default; + Profile(Profile&&) = default; + Profile& operator=(Profile&&) = default; + std::size_t key_{ 0 }; friend class boost::serialization::access; template From 40c14ab19de91530419c003f3acf5a734e00afe4 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 18 Dec 2024 10:37:58 -0600 Subject: [PATCH 12/13] Update descartes profiles (#546) --- .../descartes/CMakeLists.txt | 5 +- .../descartes/descartes_motion_planner.h | 3 - .../descartes/descartes_problem.h | 70 ---- .../descartes/deserialize.h | 56 --- .../impl/descartes_motion_planner.hpp | 207 +++------- .../descartes_default_plan_profile.hpp | 376 ++++-------------- ...descartes_ladder_graph_solver_profile.hpp} | 39 +- .../impl/profile/descartes_profile.hpp | 55 +++ .../profile/descartes_default_plan_profile.h | 49 +-- .../descartes_ladder_graph_solver_profile.h | 60 +++ .../descartes/profile/descartes_profile.h | 65 ++- .../descartes/types.h | 66 --- .../descartes/src/deserialize.cpp | 126 ------ .../descartes_default_plan_profile.cpp | 11 +- .../descartes_ladder_graph_solver_profile.cpp | 50 +++ .../src/profile/descartes_profile.cpp | 12 + .../descartes/src/serialize.cpp | 80 ---- .../test/descartes_planner_tests.cpp | 77 ++-- 18 files changed, 438 insertions(+), 969 deletions(-) delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h rename tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/{serialize.h => impl/profile/descartes_ladder_graph_solver_profile.hpp} (51%) create mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp create mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h delete mode 100644 tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h delete mode 100644 tesseract_motion_planners/descartes/src/deserialize.cpp create mode 100644 tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp delete mode 100644 tesseract_motion_planners/descartes/src/serialize.cpp diff --git a/tesseract_motion_planners/descartes/CMakeLists.txt b/tesseract_motion_planners/descartes/CMakeLists.txt index 5eafd22e6a8..9f94ee7b32c 100644 --- a/tesseract_motion_planners/descartes/CMakeLists.txt +++ b/tesseract_motion_planners/descartes/CMakeLists.txt @@ -8,11 +8,10 @@ add_library( src/descartes_collision.cpp src/descartes_collision_edge_evaluator.cpp src/descartes_robot_sampler.cpp - src/serialize.cpp - src/deserialize.cpp src/descartes_utils.cpp src/profile/descartes_profile.cpp - src/profile/descartes_default_plan_profile.cpp) + src/profile/descartes_default_plan_profile.cpp + src/profile/descartes_ladder_graph_solver_profile.cpp) target_link_libraries( ${PROJECT_NAME}_descartes PUBLIC ${PROJECT_NAME}_core diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h index fc5522ac307..63518795f13 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_motion_planner.h @@ -2,7 +2,6 @@ #define TESSERACT_MOTION_PLANNERS_DECARTES_MOTION_PLANNER_H #include -#include namespace tesseract_planning { @@ -25,8 +24,6 @@ class DescartesMotionPlanner : public MotionPlanner void clear() override; std::unique_ptr clone() const override; - - virtual std::shared_ptr> createProblem(const PlannerRequest& request) const; }; using DescartesMotionPlannerD = DescartesMotionPlanner; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h deleted file mode 100644 index aaa6c4d4890..00000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/descartes_problem.h +++ /dev/null @@ -1,70 +0,0 @@ -/** - * @file descartes_problem.h - * @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. - */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -#include - -namespace tesseract_planning -{ -template -struct DescartesProblem -{ - // LCOV_EXCL_START - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - // LCOV_EXCL_STOP - - // These are required for Tesseract to configure Descartes - std::shared_ptr env; - tesseract_scene_graph::SceneState env_state; - - // Kinematic Objects - std::shared_ptr manip; - - // These are required for descartes - std::vector::ConstPtr> edge_evaluators{}; - std::vector::ConstPtr> samplers{}; - std::vector::ConstPtr> state_evaluators{}; - int num_threads = static_cast(std::thread::hardware_concurrency()); -}; -using DescartesProblemF = DescartesProblem; -using DescartesProblemD = DescartesProblem; - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_PROBLEM_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h deleted file mode 100644 index d3d6325c773..00000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/deserialize.h +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file deserialize.h - * @brief Provide methods for deserialize descartes plans to xml - * - * @author Tyler Marr - * @date August 25, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 - -namespace tesseract_planning -{ -DescartesDefaultPlanProfile descartesPlanParser(const tinyxml2::XMLElement& xml_element); - -DescartesDefaultPlanProfile descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml); - -DescartesDefaultPlanProfile descartesPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc); - -DescartesDefaultPlanProfile descartesPlanFromXMLFile(const std::string& file_path); - -DescartesDefaultPlanProfile descartesPlanFromXMLString(const std::string& xml_string); - -} // namespace tesseract_planning - -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESERIALIZE_H 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 ade84d6c14d..b18f31cc5b4 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 @@ -40,6 +40,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -60,35 +61,67 @@ template PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& request) const { PlannerResponse response; - std::shared_ptr> problem; - if (request.data) - { - problem = std::static_pointer_cast>(request.data); - } - else + + // Get solver config + auto solver_profile = + getProfile>(name_, + request.instructions.getProfile(name_), + *request.profiles, + std::make_shared>()); + + auto solver = solver_profile->create(); + + std::vector::ConstPtr> edge_evaluators; + std::vector::ConstPtr> waypoint_samplers; + std::vector::ConstPtr> state_evaluators; + + const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); + if (composite_mi.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); + + // Flatten the input for planning + auto move_instructions = request.instructions.flatten(&moveFilter); + + // Transform plan instructions into descartes samplers + int index = 0; + for (const auto& instruction : move_instructions) { - try - { - problem = createProblem(request); - } - catch (std::exception& e) - { - CONSOLE_BRIDGE_logError("DescartesMotionPlanner failed to generate problem: %s.", e.what()); - response.successful = false; - response.message = ERROR_INVALID_INPUT; - return response; - } + assert(instruction.get().isMoveInstruction()); + const auto& move_instruction = instruction.get().template as(); + + // If plan instruction has manipulator information then use it over the one provided by the composite. + tesseract_common::ManipulatorInfo manip_info = composite_mi.getCombined(move_instruction.getManipulatorInfo()); + + if (manip_info.empty()) + throw std::runtime_error("Descartes, manipulator info is empty!"); - response.data = problem; + // Get Plan Profile + auto cur_plan_profile = + getProfile>(name_, + move_instruction.getProfile(name_), + *request.profiles, + std::make_shared>()); + + if (!cur_plan_profile) + throw std::runtime_error("DescartesMotionPlanner: Invalid profile"); + + if (move_instruction.getWaypoint().isJointWaypoint() && + !move_instruction.getWaypoint().as().isConstrained()) + continue; + + waypoint_samplers.push_back(cur_plan_profile->createWaypointSampler(move_instruction, manip_info, request.env)); + state_evaluators.push_back(cur_plan_profile->createStateEvaluator(move_instruction, manip_info, request.env)); + if (index != 0) + edge_evaluators.push_back(cur_plan_profile->createEdgeEvaluator(move_instruction, manip_info, request.env)); + + ++index; } descartes_light::SearchResult descartes_result; try { - descartes_light::LadderGraphSolver solver(problem->num_threads); - // Build Graph - if (!solver.build(problem->samplers, problem->edge_evaluators, problem->state_evaluators)) + if (!solver->build(waypoint_samplers, edge_evaluators, state_evaluators)) { response.successful = false; response.message = ERROR_FAILED_TO_BUILD_GRAPH; @@ -96,7 +129,7 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r } // Search Graph - descartes_result = solver.search(); + descartes_result = solver->search(); if (descartes_result.trajectory.empty()) { CONSOLE_BRIDGE_logError("Search for graph completion failed"); @@ -107,26 +140,17 @@ PlannerResponse DescartesMotionPlanner::solve(const PlannerRequest& r } catch (...) { - // CONSOLE_BRIDGE_logError("Failed to build vertices"); - // for (const auto& i : graph_builder.getFailedVertices()) - // response.failed_waypoints.push_back(config_->waypoints[i]); - - // // Copy the waypoint if it is not already in the failed waypoints list - // std::copy_if(config_->waypoints.begin(), - // config_->waypoints.end(), - // std::back_inserter(response.succeeded_waypoints), - // [&response](const Waypoint::ConstPtr wp) { - // return std::find(response.failed_waypoints.begin(), response.failed_waypoints.end(), wp) == - // response.failed_waypoints.end(); - // }); - response.successful = false; response.message = ERROR_FAILED_TO_BUILD_GRAPH; return response; } - const std::vector joint_names = problem->manip->getJointNames(); - const Eigen::MatrixX2d joint_limits = problem->manip->getLimits().joint_limits; + response.data = std::move(solver); + + // Get Manipulator Information + tesseract_kinematics::JointGroup::UPtr manip = request.env->getJointGroup(composite_mi.manipulator); + const std::vector joint_names = manip->getJointNames(); + const Eigen::MatrixX2d joint_limits = manip->getLimits().joint_limits; // Enforce limits std::vector solution{}; @@ -222,114 +246,5 @@ std::unique_ptr DescartesMotionPlanner::clone() const return std::make_unique>(name_); } -template -std::shared_ptr> -DescartesMotionPlanner::createProblem(const PlannerRequest& request) const -{ - auto prob = std::make_shared>(); - - // Clear descartes data - prob->edge_evaluators.clear(); - prob->samplers.clear(); - prob->state_evaluators.clear(); - - // Assume all the plan instructions have the same manipulator as the composite - assert(!request.instructions.getManipulatorInfo().empty()); - const tesseract_common::ManipulatorInfo& composite_mi = request.instructions.getManipulatorInfo(); - - if (composite_mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); - - // Get Manipulator Information - try - { - if (composite_mi.manipulator_ik_solver.empty()) - prob->manip = request.env->getKinematicGroup(composite_mi.manipulator); - else - prob->manip = request.env->getKinematicGroup(composite_mi.manipulator, composite_mi.manipulator_ik_solver); - } - catch (...) - { - throw std::runtime_error("Descartes problem generator failed to create kinematic group!"); - } - - if (!prob->manip) - { - CONSOLE_BRIDGE_logError("No Kinematics Group found"); - return prob; - } - - prob->env = request.env; - prob->env_state = request.env->getState(); - - std::vector joint_names = prob->manip->getJointNames(); - - // Flatten the input for planning - auto move_instructions = request.instructions.flatten(&moveFilter); - - // Transform plan instructions into descartes samplers - int index = 0; - for (const auto& move_instruction : move_instructions) - { - const auto& instruction = move_instruction.get(); - - assert(instruction.isMoveInstruction()); - const auto& plan_instruction = instruction.template as(); - - // If plan instruction has manipulator information then use it over the one provided by the composite. - tesseract_common::ManipulatorInfo mi = composite_mi.getCombined(plan_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); - - if (mi.tcp_frame.empty()) - throw std::runtime_error("Descartes, tcp_frame is empty!"); - - if (mi.working_frame.empty()) - throw std::runtime_error("Descartes, working_frame is empty!"); - - // Get Plan Profile - 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"); - - if (plan_instruction.getWaypoint().isCartesianWaypoint()) - { - const auto& cur_wp = plan_instruction.getWaypoint().template as(); - cur_plan_profile->apply(*prob, cur_wp.getTransform(), plan_instruction, composite_mi, index); - ++index; - } - else if (plan_instruction.getWaypoint().isJointWaypoint()) - { - if (plan_instruction.getWaypoint().as().isConstrained()) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index); - ++index; - } - } - else if (plan_instruction.getWaypoint().isStateWaypoint()) - { - assert(checkJointPositionFormat(joint_names, plan_instruction.getWaypoint())); - const Eigen::VectorXd& cur_position = getJointPosition(plan_instruction.getWaypoint()); - cur_plan_profile->apply(*prob, cur_position, plan_instruction, composite_mi, index); - ++index; - } - else - { - throw std::runtime_error("DescartesMotionPlanner: unknown waypoint type"); - } - } - - // Call the base class generate which checks the problem to make sure everything is in order - return prob; -} - } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_IMPL_DESCARTES_DECARTES_MOTION_PLANNER_HPP diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp index ba298bee380..2b83f4af782 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_default_plan_profile.hpp @@ -28,166 +28,68 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include #include #include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include #include +#include #include #include #include +#include #include #include #include #include #include +#include +#include namespace tesseract_planning { template -DescartesDefaultPlanProfile::DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element) +std::unique_ptr DescartesDefaultPlanProfile::createVertexEvaluator( + const MoveInstructionPoly& /*move_instruction*/, + const std::shared_ptr& manip, + const std::shared_ptr& /*env*/) const { - const tinyxml2::XMLElement* vertex_collisions_element = xml_element.FirstChildElement("VertexCollisions"); - const tinyxml2::XMLElement* edge_collisions_element = xml_element.FirstChildElement("EdgeCollisions"); - const tinyxml2::XMLElement* num_threads_element = xml_element.FirstChildElement("NumberThreads"); - const tinyxml2::XMLElement* allow_collisions_element = xml_element.FirstChildElement("AllowCollisions"); - const tinyxml2::XMLElement* debug_element = xml_element.FirstChildElement("Debug"); - - int status{ tinyxml2::XMLError::XML_SUCCESS }; - - if (vertex_collisions_element != nullptr) - { - const tinyxml2::XMLElement* enabled_element = vertex_collisions_element->FirstChildElement("Enabled"); - // const tinyxml2::XMLElement* coll_safety_margin_element = - // vertex_collisions_element->FirstChildElement("CollisionSaf" - // "etyMargin"); - - if (enabled_element != nullptr) - { - status = enabled_element->QueryBoolText(&enable_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: VertexCollisions: Error parsing Enabled string"); - } - - /** @todo Update XML */ - // if (coll_safety_margin_element) - // { - // std::string coll_safety_margin_string; - // status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string); - // if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - // throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: Error parsing CollisionSafetyMargin - // string"); - - // if (!tesseract_common::isNumeric(coll_safety_margin_string)) - // throw std::runtime_error("DescartesPlanProfile: VertexCollisions:: CollisionSafetyMargin is not a numeric - // " - // "values."); - - // tesseract_common::toNumeric(coll_safety_margin_string, collision_safety_margin); - // } - } - - if (edge_collisions_element != nullptr) - { - const tinyxml2::XMLElement* enabled_element = edge_collisions_element->FirstChildElement("Enabled"); - const tinyxml2::XMLElement* coll_safety_margin_element = edge_collisions_element->FirstChildElement("CollisionSafet" - "yMargin"); - const tinyxml2::XMLElement* long_valid_seg_len_element = edge_collisions_element->FirstChildElement("LongestValidSe" - "gmentLength"); - - if (enabled_element != nullptr) - { - status = enabled_element->QueryBoolText(&enable_edge_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing Enabled string"); - } - - if (coll_safety_margin_element != nullptr) - { - std::string coll_safety_margin_string; - status = tesseract_common::QueryStringText(coll_safety_margin_element, coll_safety_margin_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing CollisionSafetyMargin string"); - - if (!tesseract_common::isNumeric(coll_safety_margin_string)) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: CollisionSafetyMargin is not a numeric " - "values."); - - /** @todo Update XML */ - // tesseract_common::toNumeric(coll_safety_margin_string, edge_collision_saftey_margin); - } - - if (long_valid_seg_len_element != nullptr) - { - std::string long_valid_seg_len_string; - status = tesseract_common::QueryStringText(long_valid_seg_len_element, long_valid_seg_len_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: Error parsing LongestValidSegmentLength " - "string"); - - if (!tesseract_common::isNumeric(long_valid_seg_len_string)) - throw std::runtime_error("DescartesPlanProfile: EdgeCollisions: LongestValidSegmentLength is not a numeric " - "values."); - - /** @todo Update XML */ - // tesseract_common::toNumeric(long_valid_seg_len_string, edge_longest_valid_segment_length); - } - } - - if (num_threads_element != nullptr) - { - std::string num_threads_string; - status = tesseract_common::QueryStringText(num_threads_element, num_threads_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing NumberThreads string"); - - if (!tesseract_common::isNumeric(num_threads_string)) - throw std::runtime_error("DescartesPlanProfile: NumberThreads is not a numeric values."); - - tesseract_common::toNumeric(num_threads_string, num_threads); - } - - if (allow_collisions_element != nullptr) - { - status = allow_collisions_element->QueryBoolText(&allow_collision); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing AllowCollisions string"); - } - - if (debug_element != nullptr) - { - status = debug_element->QueryBoolText(&debug); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("DescartesPlanProfile: Error parsing Debug string"); - } + return std::make_unique(manip->getLimits().joint_limits); } template -void DescartesDefaultPlanProfile::apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const +PoseSamplerFn DescartesDefaultPlanProfile::createPoseSampler( + const MoveInstructionPoly& /*move_instruction*/, + const std::shared_ptr& /*manip*/, + const std::shared_ptr& /*env*/) const { - assert(parent_instruction.isMoveInstruction()); - const auto& base_instruction = parent_instruction.as(); - assert(!(manip_info.empty() && base_instruction.getManipulatorInfo().empty())); - tesseract_common::ManipulatorInfo mi = manip_info.getCombined(base_instruction.getManipulatorInfo()); - - if (mi.manipulator.empty()) - throw std::runtime_error("Descartes, manipulator is empty!"); + if (target_pose_fixed) + return sampleFixed; - if (mi.tcp_frame.empty()) - throw std::runtime_error("Descartes, tcp_frame is empty!"); + return [axis = target_pose_sample_axis, resolution = target_pose_sample_resolution]( + const Eigen::Isometry3d& tool_pose) { return sampleToolAxis(tool_pose, resolution, axis); }; +} - if (mi.working_frame.empty()) - throw std::runtime_error("Descartes, working_frame is empty!"); +template +std::unique_ptr> +DescartesDefaultPlanProfile::createWaypointSampler( + const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const +{ + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (!move_instruction.getWaypoint().isCartesianWaypoint()) + { + assert(checkJointPositionFormat(manip->getJointNames(), move_instruction.getWaypoint())); + const Eigen::VectorXd& joint_waypoint = getJointPosition(move_instruction.getWaypoint()); + auto state = std::make_shared>(joint_waypoint.cast()); + return std::make_unique>(state); + } - Eigen::Isometry3d tcp_offset = prob.env->findTCPOffset(mi); - std::vector joint_names = prob.manip->getJointNames(); + Eigen::Isometry3d tcp_offset = env->findTCPOffset(manip_info); /* Check if this cartesian waypoint is dynamic * (i.e. defined relative to a frame that will move with the kinematic chain) */ @@ -199,182 +101,64 @@ void DescartesDefaultPlanProfile::apply(DescartesProblem& DescartesCollision::Ptr ci = nullptr; if (enable_collision) - ci = std::make_shared(*prob.env, prob.manip, vertex_collision_check_config, debug); - - // Add vertex evaluator - std::shared_ptr> sampler; - if (vertex_evaluator == nullptr) - { - auto ve = std::make_shared(prob.manip->getLimits().joint_limits); - sampler = std::make_shared>(mi.working_frame, - cartesian_waypoint, - target_pose_sampler, - prob.manip, - ci, - mi.tcp_frame, - tcp_offset, - allow_collision, - ve, - use_redundant_joint_solutions); - } - else - { - sampler = std::make_shared>(mi.working_frame, - cartesian_waypoint, - target_pose_sampler, - prob.manip, - ci, - mi.tcp_frame, - tcp_offset, - allow_collision, - vertex_evaluator(prob), - use_redundant_joint_solutions); - } - prob.samplers.push_back(std::move(sampler)); - - if (index != 0) - { - // Add edge Evaluator - if (edge_evaluator == nullptr) - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back( - std::make_shared>()); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(std::make_shared>()); - } - } - else - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back(edge_evaluator(prob)); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(edge_evaluator(prob)); - } - } - } - - // Add state evaluator - if (state_evaluator != nullptr) - prob.state_evaluators.push_back(state_evaluator(prob)); - else - prob.state_evaluators.push_back(std::make_shared>()); - - prob.num_threads = num_threads; + ci = std::make_shared(*env, manip, vertex_collision_check_config, debug); + + auto ve = createVertexEvaluator(move_instruction, manip, env); + auto pose_sampler = createPoseSampler(move_instruction, manip, env); + return std::make_unique>( + manip_info.working_frame, + move_instruction.getWaypoint().as().getTransform(), + pose_sampler, + manip, + ci, + manip_info.tcp_frame, + tcp_offset, + allow_collision, + std::move(ve), + use_redundant_joint_solutions); } template -void DescartesDefaultPlanProfile::apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& /*parent_instruction*/, - const tesseract_common::ManipulatorInfo& /*manip_info*/, - int index) const +std::unique_ptr> DescartesDefaultPlanProfile::createEdgeEvaluator( + const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const { - auto state = std::make_shared>(joint_waypoint.cast()); - auto sampler = std::make_shared>(state); - prob.samplers.push_back(std::move(sampler)); + auto manip = DescartesPlanProfile::createKinematicGroup(manip_info, *env); + if (move_instruction.getWaypoint().isCartesianWaypoint()) + { + if (!enable_edge_collision) + return std::make_unique>(); - std::vector joint_names = prob.manip->getJointNames(); + auto compound_evaluator = std::make_unique>(); + compound_evaluator->evaluators.push_back( + std::make_shared>()); + compound_evaluator->evaluators.push_back(std::make_shared>( + *env, manip, edge_collision_check_config, allow_collision, debug)); - if (index != 0) - { - // Add edge Evaluator - if (edge_evaluator == nullptr) - { - if (enable_edge_collision) - { - auto compound_evaluator = std::make_shared>(); - compound_evaluator->evaluators.push_back( - std::make_shared>()); - compound_evaluator->evaluators.push_back(std::make_shared>( - *prob.env, prob.manip, edge_collision_check_config, allow_collision, debug)); - prob.edge_evaluators.push_back(compound_evaluator); - } - else - { - prob.edge_evaluators.push_back(std::make_shared>()); - } - } - else - { - prob.edge_evaluators.push_back(edge_evaluator(prob)); - } + return compound_evaluator; } - if (state_evaluator != nullptr) - prob.state_evaluators.push_back(state_evaluator(prob)); - else - prob.state_evaluators.push_back(std::make_shared>()); + if (!enable_edge_collision) + return std::make_unique>(); - prob.num_threads = num_threads; + auto compound_evaluator = std::make_unique>(); + compound_evaluator->evaluators.push_back( + std::make_shared>()); + compound_evaluator->evaluators.push_back(std::make_shared>( + *env, manip, edge_collision_check_config, allow_collision, debug)); + + return compound_evaluator; } template -tinyxml2::XMLElement* DescartesDefaultPlanProfile::toXML(tinyxml2::XMLDocument& doc) const +std::unique_ptr> +DescartesDefaultPlanProfile::createStateEvaluator( + const MoveInstructionPoly& /*move_instruction*/, + const tesseract_common::ManipulatorInfo& /*manip_info*/, + const std::shared_ptr& /*env*/) const { - tinyxml2::XMLElement* xml_planner = doc.NewElement("Planner"); - xml_planner->SetAttribute("type", std::to_string(3).c_str()); - - tinyxml2::XMLElement* xml_descartes = doc.NewElement("DescartesPlanProfile"); - - tinyxml2::XMLElement* vertex_collisions = doc.NewElement("VertexCollisions"); - tinyxml2::XMLElement* vertex_collisions_enabled = doc.NewElement("Enabled"); - vertex_collisions_enabled->SetText(enable_collision); - vertex_collisions->InsertEndChild(vertex_collisions_enabled); - - // tinyxml2::XMLElement* vertex_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // vertex_collisions_safety_margin->SetText(collision_safety_margin); - // vertex_collisions->InsertEndChild(vertex_collisions_safety_margin); - - xml_descartes->InsertEndChild(vertex_collisions); - - tinyxml2::XMLElement* edge_collisions = doc.NewElement("EdgeCollisions"); - tinyxml2::XMLElement* edge_collisions_enabled = doc.NewElement("Enabled"); - edge_collisions_enabled->SetText(enable_edge_collision); - edge_collisions->InsertEndChild(edge_collisions_enabled); - - /** @todo Update XML */ - // tinyxml2::XMLElement* edge_collisions_safety_margin = doc.NewElement("CollisionSafetyMargin"); - // edge_collisions_safety_margin->SetText(edge_collision_saftey_margin); - // edge_collisions->InsertEndChild(edge_collisions_safety_margin); - - // tinyxml2::XMLElement* edge_collisions_long_valid_seg_len = doc.NewElement("LongestValidSegmentLength"); - // edge_collisions_long_valid_seg_len->SetText(edge_longest_valid_segment_length); - // edge_collisions->InsertEndChild(edge_collisions_long_valid_seg_len); - - xml_descartes->InsertEndChild(edge_collisions); - - tinyxml2::XMLElement* number_threads = doc.NewElement("NumberThreads"); - number_threads->SetText(num_threads); - xml_descartes->InsertEndChild(number_threads); - - tinyxml2::XMLElement* allow_collision_element = doc.NewElement("AllowCollisions"); - allow_collision_element->SetText(allow_collision); - xml_descartes->InsertEndChild(allow_collision_element); - - tinyxml2::XMLElement* debug_element = doc.NewElement("Debug"); - debug_element->SetText(debug); - xml_descartes->InsertEndChild(debug_element); - - xml_planner->InsertEndChild(xml_descartes); - - // TODO: Add Edge Evaluator and IsValidFn? - - return xml_planner; + return std::make_unique>(); } } // namespace tesseract_planning diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp similarity index 51% rename from tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h rename to tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp index 44e485ed44a..35e26020007 100644 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/serialize.h +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_ladder_graph_solver_profile.hpp @@ -1,9 +1,9 @@ /** - * @file serialize.h - * @brief Provide methods for serializing descartes plans to xml + * @file descartes_ladder_graph_solver_profile.hpp + * @brief * - * @author Tyler Marr - * @date August 25, 2020 + * @author Levi Armstrong + * @date June 18, 2020 * @version TODO * @bug No known bugs * @@ -23,29 +23,20 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP +#define TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +#include +#include namespace tesseract_planning { -std::shared_ptr toXMLDocument(const DescartesPlanProfile& plan_profile); - -bool toXMLFile(const DescartesPlanProfile& plan_profile, const std::string& file_path); +template +std::unique_ptr> DescartesLadderGraphSolverProfile::create() const +{ + return std::make_unique>(num_threads); +} -std::string toXMLString(const DescartesPlanProfile& plan_profile); } // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_SERIALIZE_H + +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_HPP diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp new file mode 100644 index 00000000000..c76f932f00e --- /dev/null +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/impl/profile/descartes_profile.hpp @@ -0,0 +1,55 @@ +/** + * @file descartes_profile.h + * @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. + */ +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP +#define TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP +#include +#include +#include +#include + +namespace tesseract_planning +{ +template +std::shared_ptr +DescartesPlanProfile::createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_environment::Environment& env) const +{ + // Get Manipulator Information + try + { + if (manip_info.manipulator_ik_solver.empty()) + return env.getKinematicGroup(manip_info.manipulator); + + return env.getKinematicGroup(manip_info.manipulator, manip_info.manipulator_ik_solver); + } + catch (...) + { + throw std::runtime_error("Descartes problem generator failed to create kinematic group!"); + } +} + +} // namespace tesseract_planning +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_IMPL_DESCARTES_PROFILE_HPP 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 1bdde8ee7d4..db8e2ec1db1 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 @@ -33,12 +33,13 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include #include namespace tesseract_planning { +class DescartesVertexEvaluator; + template class DescartesDefaultPlanProfile : public DescartesPlanProfile { @@ -47,15 +48,10 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile using ConstPtr = std::shared_ptr>; DescartesDefaultPlanProfile() = default; - DescartesDefaultPlanProfile(const tinyxml2::XMLElement& xml_element); - - PoseSamplerFn target_pose_sampler = sampleFixed; - - DescartesEdgeEvaluatorAllocatorFn edge_evaluator{ nullptr }; - DescartesStateEvaluatorAllocatorFn state_evaluator{ nullptr }; - // If not provided it adds a joint limit is valid function - DescartesVertexEvaluatorAllocatorFn vertex_evaluator{ nullptr }; + bool target_pose_fixed{ true }; + Eigen::Vector3d target_pose_sample_axis{ 0, 0, 1 }; + double target_pose_sample_resolution{ M_PI_2 }; /** * @brief Flag to indicate that collisions should not cause failures during state/edge evaluation @@ -79,30 +75,37 @@ class DescartesDefaultPlanProfile : public DescartesPlanProfile */ bool use_redundant_joint_solutions{ false }; - /** @brief Number of threads to use during planning */ - int num_threads{ 1 }; - /** @brief Flag to produce debug information during planning */ bool debug{ false }; - void apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const override; + std::unique_ptr> + createWaypointSampler(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; - void apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const override; + std::unique_ptr> + createEdgeEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; - tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override; + std::unique_ptr> + createStateEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const override; protected: friend class boost::serialization::access; template void serialize(Archive&, const unsigned int); // NOLINT + + virtual std::unique_ptr + createVertexEvaluator(const MoveInstructionPoly& move_instruction, + const std::shared_ptr& manip, + const std::shared_ptr& env) const; + + virtual PoseSamplerFn createPoseSampler(const MoveInstructionPoly& move_instruction, + const std::shared_ptr& manip, + const std::shared_ptr& env) const; }; using DescartesDefaultPlanProfileF = DescartesDefaultPlanProfile; diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h new file mode 100644 index 00000000000..6f7da3e2186 --- /dev/null +++ b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/profile/descartes_ladder_graph_solver_profile.h @@ -0,0 +1,60 @@ +/** + * @file descartes_ladder_graph_solver_profile.h + * @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. + */ +#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_H +#define TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_PROFILE_H + +#include + +namespace tesseract_planning +{ +template +class DescartesLadderGraphSolverProfile : public DescartesSolverProfile +{ +public: + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; + + DescartesLadderGraphSolverProfile() = default; + + /** @brief Number of threads to use during planning */ + int num_threads{ 1 }; + + std::unique_ptr> create() const override; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive&, const unsigned int); // NOLINT +}; + +using DescartesLadderGraphSolverProfileF = DescartesLadderGraphSolverProfile; +using DescartesLadderGraphSolverProfileD = DescartesLadderGraphSolverProfile; +} // namespace tesseract_planning + +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_KEY(tesseract_planning::DescartesLadderGraphSolverProfile) + +#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_LADDER_GRAPH_SOLVER_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 8f90e755ff3..81d3b12fdd5 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 @@ -35,17 +35,40 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include -#include -#include +#include +#include -namespace tinyxml2 -{ -class XMLElement; // NOLINT -class XMLDocument; -} // namespace tinyxml2 +#include +#include +#include +#include +#include namespace tesseract_planning { +template +class DescartesSolverProfile : public Profile +{ +public: + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; + + DescartesSolverProfile() : Profile(DescartesSolverProfile::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(DescartesSolverProfile)).hash_code(); } + + virtual std::unique_ptr> create() const = 0; + +protected: + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int); // NOLINT +}; + template class DescartesPlanProfile : public Profile { @@ -61,19 +84,24 @@ class DescartesPlanProfile : public Profile */ static std::size_t getStaticKey() { return std::type_index(typeid(DescartesPlanProfile)).hash_code(); } - virtual void apply(DescartesProblem& prob, - const Eigen::Isometry3d& cartesian_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const = 0; + std::shared_ptr + createKinematicGroup(const tesseract_common::ManipulatorInfo& manip_info, + const tesseract_environment::Environment& env) const; + + virtual std::unique_ptr> + createWaypointSampler(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; - virtual void apply(DescartesProblem& prob, - const Eigen::VectorXd& joint_waypoint, - const InstructionPoly& parent_instruction, - const tesseract_common::ManipulatorInfo& manip_info, - int index) const = 0; + virtual std::unique_ptr> + createEdgeEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; - virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0; + virtual std::unique_ptr> + createStateEvaluator(const MoveInstructionPoly& move_instruction, + const tesseract_common::ManipulatorInfo& manip_info, + const std::shared_ptr& env) const = 0; protected: friend class boost::serialization::access; @@ -81,7 +109,6 @@ class DescartesPlanProfile : public Profile 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 */ } // namespace tesseract_planning #endif // TESSERACT_MOTION_PLANNERS_DESCARTES_DESCARTES_PROFILE_H diff --git a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h b/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h deleted file mode 100644 index e9cc809c1f9..00000000000 --- a/tesseract_motion_planners/descartes/include/tesseract_motion_planners/descartes/types.h +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file types.h - * @brief Tesseract descartes types - * - * @author Levi Armstrong - * @date April 18, 2018 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2017, 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_DESCARTES_TYPES_H -#define TESSERACT_MOTION_PLANNERS_DESCARTES_TYPES_H - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -/** - * @brief This is used for tesseract descartes samplers that filters out invalid solutions. - * - * Example: This would be used to filter out solution outside of custom joint limits. - * - */ -template -using DescartesVertexEvaluatorAllocatorFn = - std::function&)>; - -/** - * @brief This is used to create edge evaluator within tesseract, to allow thread safe creation of descartes edge - * evaluators - */ -template -using DescartesEdgeEvaluatorAllocatorFn = - std::function::Ptr(const DescartesProblem&)>; - -/** - * @brief Creates a state evaluator - */ -template -using DescartesStateEvaluatorAllocatorFn = - std::function::Ptr(const DescartesProblem&)>; - -} // namespace tesseract_planning -#endif // TESSERACT_MOTION_PLANNERS_DESCARTES_TYPES_H diff --git a/tesseract_motion_planners/descartes/src/deserialize.cpp b/tesseract_motion_planners/descartes/src/deserialize.cpp deleted file mode 100644 index 0214f2f3ca3..00000000000 --- a/tesseract_motion_planners/descartes/src/deserialize.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file deserialize.cpp - * @brief Provide methods for deserialize instructions to xml and deserialization - * - * @author Tyler Marr - * @date August 21, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include -#include - -namespace tesseract_planning -{ -DescartesDefaultPlanProfile descartesPlanParser(const tinyxml2::XMLElement& xml_element) -{ - const tinyxml2::XMLElement* descartes_plan_element = xml_element.FirstChildElement("DescartesPlanProfile"); - return { *descartes_plan_element }; -} - -DescartesDefaultPlanProfile descartesPlanFromXMLElement(const tinyxml2::XMLElement* profile_xml) -{ - std::array version{ 0, 0, 0 }; - std::string version_string; - int status = tesseract_common::QueryStringAttribute(profile_xml, "version", version_string); - if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - if (status != tinyxml2::XML_NO_ATTRIBUTE) - { - std::vector tokens; - boost::split(tokens, version_string, boost::is_any_of("."), boost::token_compress_on); - if (tokens.size() < 2 || tokens.size() > 3 || !tesseract_common::isNumeric(tokens)) - throw std::runtime_error("fromXML: Error parsing robot attribute 'version'"); - - tesseract_common::toNumeric(tokens[0], version[0]); - tesseract_common::toNumeric(tokens[1], version[1]); - if (tokens.size() == 3) - tesseract_common::toNumeric(tokens[2], version[2]); - else - version[2] = 0; - } - else - { - CONSOLE_BRIDGE_logWarn("No version number was provided so latest parser will be used."); - } - - const tinyxml2::XMLElement* planner_xml = profile_xml->FirstChildElement("Planner"); - if (planner_xml == nullptr) - throw std::runtime_error("fromXML: Could not find the 'Planner' element in the xml file."); - - int type{ 0 }; - status = planner_xml->QueryIntAttribute("type", &type); - if (status != tinyxml2::XML_SUCCESS) - throw std::runtime_error("fromXML: Failed to parse instruction type attribute."); - - return descartesPlanParser(*planner_xml); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLDocument(const tinyxml2::XMLDocument& xml_doc) -{ - const tinyxml2::XMLElement* planner_xml = xml_doc.FirstChildElement("Profile"); - if (planner_xml == nullptr) - throw std::runtime_error("Could not find the 'Profile' element in the xml file"); - - return descartesPlanFromXMLElement(planner_xml); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLString(const std::string& xml_string) -{ - tinyxml2::XMLDocument xml_doc; - int status = xml_doc.Parse(xml_string.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - throw std::runtime_error("Could not parse the Planner Profile XML File."); - - return descartesPlanFromXMLDocument(xml_doc); -} - -DescartesDefaultPlanProfile descartesPlanFromXMLFile(const std::string& file_path) -{ - // get the entire file - std::string xml_string; - std::fstream xml_file(file_path.c_str(), std::fstream::in); - if (xml_file.is_open()) - { - while (xml_file.good()) - { - std::string line; - std::getline(xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - return descartesPlanFromXMLString(xml_string); - } - - throw std::runtime_error("Could not open file " + file_path + "for parsing."); -} - -} // namespace tesseract_planning 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 70d073c23bd..63fbe277348 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,7 +24,9 @@ * limitations under the License. */ #include +#include #include +#include #include #include #include @@ -40,18 +42,15 @@ 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(target_pose_fixed); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_axis); + ar& BOOST_SERIALIZATION_NVP(target_pose_sample_resolution); 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); } diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp new file mode 100644 index 00000000000..1d0a1012671 --- /dev/null +++ b/tesseract_motion_planners/descartes/src/profile/descartes_ladder_graph_solver_profile.cpp @@ -0,0 +1,50 @@ +/** + * @file descartes_ladder_graph_solver_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 +{ +// Explicit template instantiation +template class DescartesLadderGraphSolverProfile; +template class DescartesLadderGraphSolverProfile; + +template +template +void DescartesLadderGraphSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(DescartesSolverProfile); + ar& BOOST_SERIALIZATION_NVP(num_threads); +} + +} // namespace tesseract_planning + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesLadderGraphSolverProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesLadderGraphSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesLadderGraphSolverProfile) diff --git a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp index 0eeffa0a126..b3a6e6ff37b 100644 --- a/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp +++ b/tesseract_motion_planners/descartes/src/profile/descartes_profile.cpp @@ -29,6 +29,13 @@ namespace tesseract_planning { +template +template +void DescartesSolverProfile::serialize(Archive& ar, const unsigned int /*version*/) +{ + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Profile); +} + template template void DescartesPlanProfile::serialize(Archive& ar, const unsigned int /*version*/) @@ -43,3 +50,8 @@ TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfil BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesPlanProfile) BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesPlanProfile) + +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesSolverProfile) +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::DescartesSolverProfile) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::DescartesSolverProfile) diff --git a/tesseract_motion_planners/descartes/src/serialize.cpp b/tesseract_motion_planners/descartes/src/serialize.cpp deleted file mode 100644 index 38be671f737..00000000000 --- a/tesseract_motion_planners/descartes/src/serialize.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/** - * @file serialize.cpp - * @brief - * - * @author Tyler Marr - * @date August 25, 2020 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -TESSERACT_COMMON_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -#include -TESSERACT_COMMON_IGNORE_WARNINGS_POP - -#include - -static const std::array VERSION{ { 1, 0, 0 } }; -static const Eigen::IOFormat eigen_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " "); - -namespace tesseract_planning -{ -std::shared_ptr toXMLDocument(const DescartesPlanProfile& plan_profile) -{ - auto doc = std::make_shared(); - tinyxml2::XMLElement* xml_root = doc->NewElement("Profile"); - xml_root->SetAttribute("name", "DescartesDefaultProfile"); - xml_root->SetAttribute( - "version", - (std::to_string(VERSION[0]) + "." + std::to_string(VERSION[1]) + "." + std::to_string(VERSION[2])).c_str()); - - tinyxml2::XMLElement* xml_plan_profile = plan_profile.toXML(*doc); - xml_root->InsertEndChild(xml_plan_profile); - doc->InsertFirstChild(xml_root); - - return doc; -} - -bool toXMLFile(const DescartesPlanProfile& plan_profile, const std::string& file_path) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLError status = doc->SaveFile(file_path.c_str()); - if (status != tinyxml2::XMLError::XML_SUCCESS) - { - CONSOLE_BRIDGE_logError("Failed to save Plan Profile XML File: %s", file_path.c_str()); - return false; - } - - return true; -} - -std::string toXMLString(const DescartesPlanProfile& plan_profile) -{ - std::shared_ptr doc = toXMLDocument(plan_profile); - tinyxml2::XMLPrinter printer; - doc->Print(&printer); - return { printer.CStr() }; -} - -} // namespace tesseract_planning diff --git a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp index 9debf0705e0..16299b953c0 100644 --- a/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp +++ b/tesseract_motion_planners/descartes/test/descartes_planner_tests.cpp @@ -47,8 +47,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include -#include +#include #include #include #include @@ -89,24 +88,6 @@ class TesseractPlanningDescartesUnit : public ::testing::Test } }; -TEST(TesseractPlanningDescartesSerializeUnit, SerializeDescartesDefaultPlanToXml) // NOLINT -{ - // Write program to file - DescartesDefaultPlanProfile plan_profile; - plan_profile.enable_edge_collision = true; - - EXPECT_TRUE(toXMLFile(plan_profile, tesseract_common::getTempPath() + "descartes_default_plan_example_input.xml")); - - // Import file - DescartesDefaultPlanProfile imported_plan_profile = - descartesPlanFromXMLFile(tesseract_common::getTempPath() + "descartes_default_plan_example_input.xml"); - - // Re-write file and compare changed from default term - EXPECT_TRUE( - toXMLFile(imported_plan_profile, tesseract_common::getTempPath() + "descartes_default_plan_example_input2.xml")); - EXPECT_TRUE(plan_profile.enable_edge_collision == imported_plan_profile.enable_edge_collision); -} - TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT { // Specify a start waypoint @@ -133,15 +114,18 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + 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", solver_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; // Create Planning Request PlannerRequest request; @@ -156,15 +140,8 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerFixedPoses) // NOLINT for (int i = 0; i < 10; ++i) { - // Test the problem generator - { - auto problem = single_descartes_planner.createProblem(request); - EXPECT_EQ(problem->samplers.size(), 11); - EXPECT_EQ(problem->edge_evaluators.size(), 10); - } - DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); @@ -236,19 +213,23 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 10); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + auto plan_profile = std::make_shared(); + // Make this a tool z-axis free sampler - plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& tool_pose) { - return tesseract_planning::sampleToolAxis(tool_pose, M_PI_4, Eigen::Vector3d(0, 0, 1)); - }; + plan_profile->target_pose_fixed = false; + plan_profile->target_pose_sample_axis = Eigen::Vector3d(0, 0, 1); + plan_profile->target_pose_sample_resolution = M_PI_4; // Profile Dictionary auto profiles = std::make_shared(); profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile); + profiles->addProfile(DESCARTES_DEFAULT_NAMESPACE, "TEST_PROFILE", solver_profile); // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; // Create Planning Request PlannerRequest request; @@ -256,11 +237,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT request.env = env_; request.profiles = profiles; - auto problem = single_descartes_planner.createProblem(request); - problem->num_threads = 1; - EXPECT_EQ(problem->samplers.size(), 11); - EXPECT_EQ(problem->edge_evaluators.size(), 10); - PlannerResponse single_planner_response = single_descartes_planner.solve(request); EXPECT_TRUE(&single_planner_response); @@ -269,7 +245,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerAxialSymetric) // NOLINT for (int i = 0; i < 10; ++i) { DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); EXPECT_TRUE(&planner_response); @@ -330,12 +306,18 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) CompositeInstruction interpolated_program = generateInterpolatedProgram(program, env_, 3.14, 1.0, 3.14, 2); // Create Profiles + auto solver_profile = std::make_shared(); + solver_profile->num_threads = 1; + auto plan_profile = std::make_shared(); + // Make this a tool z-axis free sampler - plan_profile->target_pose_sampler = [](const Eigen::Isometry3d& tool_pose) { - return tesseract_planning::sampleToolAxis(tool_pose, 60 * M_PI * 180.0, Eigen::Vector3d(0, 0, 1)); - }; - plan_profile->enable_edge_collision = true; // Add collision edge evaluator + plan_profile->target_pose_fixed = false; + plan_profile->target_pose_sample_axis = Eigen::Vector3d(0, 0, 1); + plan_profile->target_pose_sample_resolution = 60 * M_PI * 180.0; + + // Add collision edge evaluator + plan_profile->enable_edge_collision = true; // Profile Dictionary auto profiles = std::make_shared(); @@ -349,13 +331,6 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) // Create Planner DescartesMotionPlannerD single_descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 1; - - // Test Problem size - TODO: Make dedicated unit test for DefaultDescartesProblemGenerator - auto problem = single_descartes_planner.createProblem(request); - EXPECT_EQ(problem->samplers.size(), 3); - EXPECT_EQ(problem->edge_evaluators.size(), 2); - EXPECT_EQ(problem->num_threads, 1); PlannerResponse single_planner_response = single_descartes_planner.solve(request); EXPECT_TRUE(&single_planner_response); @@ -365,7 +340,7 @@ TEST_F(TesseractPlanningDescartesUnit, DescartesPlannerCollisionEdgeEvaluator) for (int i = 0; i < 10; ++i) { DescartesMotionPlannerD descartes_planner(DESCARTES_DEFAULT_NAMESPACE); - plan_profile->num_threads = 4; + solver_profile->num_threads = 4; PlannerResponse planner_response = descartes_planner.solve(request); EXPECT_TRUE(&planner_response); From 12465394cb8039c1f7992f12971edc6d22e0305b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 18 Dec 2024 13:37:59 -0600 Subject: [PATCH 13/13] Fixed clang-format in command language test suite --- .../cartesian_waypoint_poly_unit.hpp | 303 ++++++------ .../test_suite/joint_waypoint_poly_unit.hpp | 409 ++++++++-------- .../test_suite/state_waypoint_poly_unit.hpp | 447 +++++++++--------- 3 files changed, 581 insertions(+), 578 deletions(-) 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 73e0be72e7a..e599918da6c 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,168 +63,169 @@ 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()); -} - -{ // 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()); -} + pose.translation() = Eigen::Vector3d(1, 2, 3); + CartesianWaypointPoly wp{ T(pose) }; + EXPECT_TRUE(wp.getTransform().isApprox(pose)); + EXPECT_FALSE(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()); - - wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 5)); - wp.setLowerTolerance(Eigen::VectorXd::Constant(3, -5)); - EXPECT_TRUE(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()); - 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 -{ // 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 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); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { // 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 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 clear seed - CartesianWaypointPoly wp{ T() }; - EXPECT_FALSE(wp.hasSeed()); - wp.getSeed() = seed_state; - EXPECT_TRUE(wp.hasSeed()); - wp.clearSeed(); - EXPECT_FALSE(wp.hasSeed()); + { // 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()); + } } } -} } // 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 5353ab1545b..ca119e4d089 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,227 +52,228 @@ 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 - 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 + { // Test is constrained JointWaypointPoly wp{ T() }; - wp.setNames(names); - EXPECT_TRUE(wp.getNames() == names); + EXPECT_FALSE(wp.isConstrained()); + wp.setIsConstrained(true); + EXPECT_TRUE(wp.isConstrained()); } - { // Test assigning - JointWaypointPoly 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 + 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); + } } -} -{ // Set/Get Positions - Eigen::VectorXd positions; - positions.resize(3); - positions << 1.0, 2.0, 3.0; + { // 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)); - } + { // Test set + JointWaypointPoly wp{ T() }; + wp.setPosition(positions); + EXPECT_TRUE(wp.getPosition().isApprox(positions)); + } - { // Test assigning - JointWaypointPoly wp{ T() }; - wp.getPosition() = positions; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + { // Test assigning + JointWaypointPoly wp{ T() }; + wp.getPosition() = positions; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(positions)); + } } -} - -{ // 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_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 -{ // 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); + wp.setUpperTolerance(Eigen::VectorXd::Constant(3, 0)); + wp.setLowerTolerance(Eigen::VectorXd::Constant(3, 0)); + EXPECT_FALSE(wp.isToleranced()); } - { - 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); + { // 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); + } } } -} } // 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/state_waypoint_poly_unit.hpp b/tesseract_command_language/include/tesseract_command_language/test_suite/state_waypoint_poly_unit.hpp index 08a14b851a8..991a77d5c57 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,256 +52,257 @@ 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 + { // 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); - } + { // Set/Get Names + const std::vector names{ "j1", "j2", "j3" }; + { // Test set + StateWaypointPoly wp{ T() }; + wp.setNames(names); + EXPECT_TRUE(wp.getNames() == names); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getNames() = names; - EXPECT_TRUE(std::as_const(wp).getNames() == names); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getNames() = names; + EXPECT_TRUE(std::as_const(wp).getNames() == names); + } } -} -{ // Set/Get Positions - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Positions + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setPosition(data); - EXPECT_TRUE(wp.getPosition().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setPosition(data); + EXPECT_TRUE(wp.getPosition().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getPosition() = data; - EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getPosition() = data; + EXPECT_TRUE(std::as_const(wp).getPosition().isApprox(data)); + } } -} -{ // Set/Get Velocity - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Velocity + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setVelocity(data); - EXPECT_TRUE(wp.getVelocity().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setVelocity(data); + EXPECT_TRUE(wp.getVelocity().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getVelocity() = data; - EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getVelocity() = data; + EXPECT_TRUE(std::as_const(wp).getVelocity().isApprox(data)); + } } -} -{ // Set/Get Acceleration - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Acceleration + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setAcceleration(data); - EXPECT_TRUE(wp.getAcceleration().isApprox(data)); - } + { // Test set + StateWaypointPoly wp{ T() }; + wp.setAcceleration(data); + EXPECT_TRUE(wp.getAcceleration().isApprox(data)); + } - { // Test assigning - StateWaypointPoly wp{ T() }; - wp.getAcceleration() = data; - EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getAcceleration() = data; + EXPECT_TRUE(std::as_const(wp).getAcceleration().isApprox(data)); + } } -} -{ // Set/Get Effort - Eigen::VectorXd data; - data.resize(3); - data << 1.0, 2.0, 3.0; + { // Set/Get Effort + Eigen::VectorXd data; + data.resize(3); + data << 1.0, 2.0, 3.0; - { // Test set - StateWaypointPoly wp{ T() }; - wp.setEffort(data); - EXPECT_TRUE(wp.getEffort().isApprox(data)); + { // Test set + StateWaypointPoly wp{ T() }; + wp.setEffort(data); + EXPECT_TRUE(wp.getEffort().isApprox(data)); + } + + { // Test assigning + StateWaypointPoly wp{ T() }; + wp.getEffort() = data; + EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + } } - { // Test assigning + { // Set/Get Time + double data{ 3 }; StateWaypointPoly wp{ T() }; - wp.getEffort() = data; - EXPECT_TRUE(std::as_const(wp).getEffort().isApprox(data)); + wp.setTime(data); + EXPECT_TRUE(tesseract_common::almostEqualRelativeAndAbs(wp.getTime(), 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