From 8729b17972fc437ddbca705937b480f05c8f0916 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 7 Dec 2024 11:04:58 -0600 Subject: [PATCH] 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 475d42b69d..a76c0cc8da 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 c3126ba9b6..50caae6f3c 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 0058c670db..8db53b945a 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 f3e58c4ba5..6c7548fcb2 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 025d64307b..56361187b3 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 0000000000..10eceaa3d3 --- /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 c0b3e9dc08..9d1c63ec3a 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 101297b736..e68a6a8f97 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 f1918ce42a..b44533789c 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 30f3ea26b5..c27f9660ac 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 910b688096..557656d6f4 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 e2e9f0910c..2d8d816ea2 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 9d0eb86da1..a9ed459674 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 47432d4998..a96f540774 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 45a664f8c3..0000000000 --- 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 7df99ed6e3..105fc082f5 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 019612e281..565f1c4280 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 c235fb9a53..d8f9993dc3 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 0554bc7565..5548a23ae8 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 7bc7324499..2532d3fdd4 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 e8a29f3619..a3f164a3b0 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 bb711147f5..81c3ff9934 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 2cd3e796e1..022c3de54d 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 2b6ad5310c..869b3341d7 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 52dc912280..afc5658b3c 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 59869e1a16..40393ff49b 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 bb3a124f15..1302fc9c93 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 fccab3a9da..764eb0148a 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 464e98ecc7..4d36ecd712 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 44e67878a0..d54ab165a4 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 8c80cc0dcf..5653d197ef 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 7988cbeb72..62b295f8d1 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 3947c5b8ad..34ecc3ece8 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 2afc4c8499..d26b96e686 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 ab0d2b9e5b..b4e30e6091 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 d0873270f7..a4a693718f 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 8ed42834fb..e55906e8ee 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 7d29211e11..ee0669f52e 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 918da768db..0e64f9b935 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 909a496825..af895391bb 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 99c4f7d558..eb259369f8 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 3202ee2cef..2da94de1bd 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 edbb07b212..6f4c172dbd 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 936d983e21..c071fcba39 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 32a5a0fa4e..352e6c86bd 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 a74dac807d..a6d27093ba 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 19f118ef91..5a01519f3e 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 3704292f0b..309ea0eb42 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 927cfd6abc..17276da613 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 faf33ef9f2..4ced5c32b7 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);