From e4c885f351f078110800bd8d7bb461e0bdd489da Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 18 Mar 2023 22:25:49 -0500 Subject: [PATCH 1/4] Add contact results class --- .../bullet/src/bullet_utils.cpp | 10 +- .../include/tesseract_collision/core/common.h | 8 - .../tesseract_collision/core/serialization.h | 8 + .../include/tesseract_collision/core/types.h | 141 +++++- .../benchmarks/large_dataset_benchmarks.hpp | 10 +- .../collision_box_box_cast_unit.hpp | 2 +- .../test_suite/collision_box_box_unit.hpp | 14 +- .../test_suite/collision_box_capsule_unit.hpp | 8 +- .../test_suite/collision_box_cone_unit.hpp | 8 +- .../collision_box_cylinder_unit.hpp | 8 +- .../test_suite/collision_box_sphere_unit.hpp | 16 +- .../test_suite/collision_clone_unit.hpp | 4 +- .../collision_compound_compound_unit.hpp | 4 +- .../collision_large_dataset_unit.hpp | 5 +- .../test_suite/collision_mesh_mesh_unit.hpp | 11 +- .../collision_multi_threaded_unit.hpp | 2 +- .../collision_octomap_mesh_unit.hpp | 2 +- .../collision_octomap_octomap_unit.hpp | 4 +- .../collision_octomap_sphere_unit.hpp | 2 +- .../collision_sphere_sphere_cast_unit.hpp | 16 +- .../collision_sphere_sphere_unit.hpp | 17 +- .../contact_manager_config_unit.hpp | 16 +- tesseract_collision/core/src/common.cpp | 38 +- .../core/src/serialization.cpp | 26 ++ tesseract_collision/core/src/types.cpp | 215 ++++++++-- .../examples/box_box_example.cpp | 8 +- tesseract_collision/fcl/src/fcl_utils.cpp | 12 +- .../test/benchmarks/collision_profile.cpp | 14 +- .../test/collision_core_unit.cpp | 404 +++++++++++++++++- .../allowed_collision_matrix.h | 3 +- .../tesseract_common/collision_margin_data.h | 5 +- .../include/tesseract_common/types.h | 1 + tesseract_common/src/types.cpp | 21 +- .../include/tesseract_environment/utils.h | 58 +-- tesseract_environment/src/utils.cpp | 348 +++++++-------- .../test/tesseract_environment_unit.cpp | 64 +-- .../test/tesseract_environment_utils.cpp | 56 ++- 37 files changed, 1083 insertions(+), 506 deletions(-) diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index fb943c3775e..625b0a73d79 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -688,10 +688,10 @@ btScalar addDiscreteSingleResult(btManifoldPoint& cp, const auto* cd0 = static_cast(colObj0Wrap->getCollisionObject()); // NOLINT const auto* cd1 = static_cast(colObj1Wrap->getCollisionObject()); // NOLINT - ObjectPairKey pc = getObjectPairKey(cd0->getName(), cd1->getName()); + ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd0->getName(), cd1->getName()); - const auto& it = collisions.res->find(pc); - bool found = (it != collisions.res->end()); + const auto it = collisions.res->find(pc); + bool found = (it != collisions.res->end() && !it->second.empty()); // size_t l = 0; // if (found) @@ -823,8 +823,8 @@ btScalar addCastSingleResult(btManifoldPoint& cp, std::make_pair(cd0->getName(), cd1->getName()) : std::make_pair(cd1->getName(), cd0->getName()); - auto it = collisions.res->find(pc); - bool found = it != collisions.res->end(); + const auto it = collisions.res->find(pc); + bool found = (it != collisions.res->end() && !it->second.empty()); // size_t l = 0; // if (found) diff --git a/tesseract_collision/core/include/tesseract_collision/core/common.h b/tesseract_collision/core/include/tesseract_collision/core/common.h index a6c08c14648..c3aae8b3170 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/common.h +++ b/tesseract_collision/core/include/tesseract_collision/core/common.h @@ -39,14 +39,6 @@ namespace tesseract_collision { using ObjectPairKey = std::pair; -/** - * @brief Get a key for two object to search the collision matrix - * @param obj1 First collision object name - * @param obj2 Second collision object name - * @return The collision pair key - */ -ObjectPairKey getObjectPairKey(const std::string& obj1, const std::string& obj2); - /** * @brief Get a vector of possible collision object pairs * @todo Should this also filter out links without geometry? diff --git a/tesseract_collision/core/include/tesseract_collision/core/serialization.h b/tesseract_collision/core/include/tesseract_collision/core/serialization.h index 7284575f609..ce785e75c50 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/serialization.h +++ b/tesseract_collision/core/include/tesseract_collision/core/serialization.h @@ -56,6 +56,14 @@ void load(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int template void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigned int version); // NOLINT +template +void save(Archive& ar, const tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT + +template +void load(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT + +template +void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version); // NOLINT } // namespace boost::serialization #endif // TESSERACT_COLLISION_SERIALIZATION_H diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 8c249220593..e2e6b09506d 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -132,7 +132,137 @@ struct ContactResult }; using ContactResultVector = tesseract_common::AlignedVector; -using ContactResultMap = tesseract_common::AlignedMap, ContactResultVector>; +class ContactResultMap +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using KeyType = std::pair; + using MappedType = ContactResultVector; + using ContainerType = tesseract_common::AlignedMap; + using ConstReferenceType = typename tesseract_common::AlignedMap::const_reference; + using ConstIteratorType = typename tesseract_common::AlignedMap::const_iterator; + using PairType = typename std::pair; + using FilterFn = std::function; + + /** + * @brief Add contact results for the provided key + * @param key The key to append the results to + * @param result The results to add + */ + ContactResult& addContactResult(const KeyType& key, ContactResult result); + + /** + * @brief Add contact results for the provided key + * @param key The key to append the results to + * @param result The results to add + */ + ContactResult& addContactResult(const KeyType& key, const MappedType& results); + + /** + * @brief Set contact results for the provided key + * @param key The key to append the results to + * @param result The results to add + */ + ContactResult& setContactResult(const KeyType& key, ContactResult result); + + /** + * @brief Set contact results for the provided key + * @param key The key to append the results to + * @param result The results to add + */ + ContactResult& setContactResult(const KeyType& key, const MappedType& results); + + /** + * @brief This processes interpolated contact results by updating the cc_time and cc_type and then adds the result + * @details This is copied from the trajopt utility processInterpolatedCollisionResults + * @param sub_segment_results The interpolated results to process + * @param sub_segment_index The current sub segment index + * @param sub_segment_last_index The last sub segment index + * @param active_link_names The active link names + * @param segment_dt The segment dt + * @param discrete If discrete contact checker was used + * @param filter An option filter to exclude results + */ + void addInterpolatedCollisionResults(ContactResultMap& sub_segment_results, + int sub_segment_index, + int sub_segment_last_index, + const std::vector& active_link_names, + double segment_dt, + bool discrete, + const ContactResultMap::FilterFn& filter = nullptr); + + // Flatten functions + void flattenMoveResults(ContactResultVector& v); + void flattenCopyResults(ContactResultVector& v) const; + void flattenWrapperResults(std::vector>& v); + void flattenWrapperResults(std::vector>& v) const; + + /** + * @brief Filter out results using the provided function + * @param fn The filter function + */ + void filter(const FilterFn& filter); + + /** + * @brief Get the total number of contact results storted + * @return The number of contact results + */ + long count() const; + + /** + * @brief Get the size of the map + * @details This loops over the internal map and counts entries with contacts + * @return The number of entries with contacts + */ + std::size_t size() const; + + /** + * @brief Check if results are present + * @return + */ + bool empty() const; + + /** + * @brief This is a consurvative clear. + * @details This does not call clear on the internal map but instead loops over each link pair entry and calls clear + * on the underlying vector. This way the vector capacity remains the same to avoid uneccessary heap allocation for + * subsequent contact requests. + * @note Use release to fully clear the internal data structure + */ + void clear(); + + /** @brief Fully clear all internal data */ + void release(); + + /** + * @brief Get the underlying container + * @warning Do not use this for anything other than debugging or serialization + */ + const ContainerType& getContainer() const; + + /////////////// + // Iterators // + /////////////// + /** @brief returns an iterator to the beginning */ + ConstIteratorType begin() const; + /** @brief returns an iterator to the end */ + ConstIteratorType end() const; + /** @brief returns an iterator to the beginning */ + ConstIteratorType cbegin() const; + /** @brief returns an iterator to the end */ + ConstIteratorType cend() const; + + //////////////////// + // Element Access // + //////////////////// + /** @brief access specified element with bounds checking */ + const ContactResultVector& at(const KeyType& key) const; + ConstIteratorType find(const KeyType& key) const; + +private: + ContainerType data_; + long cnt_{ 0 }; +}; /** * @brief Should return true if contact results are valid, otherwise false. @@ -163,15 +293,6 @@ struct ContactRequest ContactRequest(ContactTestType type = ContactTestType::ALL); }; -std::size_t flattenMoveResults(ContactResultMap&& m, ContactResultVector& v); - -std::size_t flattenCopyResults(const ContactResultMap& m, ContactResultVector& v); - -std::size_t flattenWrapperResults(ContactResultMap& m, std::vector>& v); - -std::size_t flattenWrapperResults(const ContactResultMap& m, - std::vector>& v); - /** * @brief This data is intended only to be used internal to the collision checkers as a container and should not * be externally used by other libraries or packages. diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/large_dataset_benchmarks.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/large_dataset_benchmarks.hpp index d13d06fe8de..4ff5e7394a5 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/large_dataset_benchmarks.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/benchmarks/large_dataset_benchmarks.hpp @@ -87,14 +87,15 @@ static void BM_LARGE_DATASET_MULTILINK(benchmark::State& state, checker->setCollisionMarginData(CollisionMarginData(0.1)); checker->setCollisionObjectsTransform(location); + ContactResultMap result; ContactResultVector result_vector; for (auto _ : state) // NOLINT { - ContactResultMap result; + result.clear(); result_vector.clear(); checker->contactTest(result, ContactTestType::ALL); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); } } @@ -181,14 +182,15 @@ static void BM_LARGE_DATASET_SINGLELINK(benchmark::State& state, checker->setCollisionMarginData(CollisionMarginData(0.1)); // checker->setCollisionObjectsTransform(location); + ContactResultMap result; ContactResultVector result_vector; for (auto _ : state) // NOLINT { - ContactResultMap result; + result.clear(); result_vector.clear(); checker->contactTest(result, ContactTestType::ALL); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); } } diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp index 65c83de9bb6..3f0f5756932 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_cast_unit.hpp @@ -160,7 +160,7 @@ inline void runTest(ContinuousContactManager& checker) checker.contactTest(result, ContactRequest(t)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.2475, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_unit.hpp index b8118b9a00c..faaa3b9c7ac 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_box_unit.hpp @@ -151,7 +151,7 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t checker.contactTest(result, ContactRequest(test_type)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -1.30, 0.001); @@ -183,7 +183,6 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t //////////////////////////////////////////////// { location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -192,7 +191,7 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t tesseract_common::VectorIsometry3d transforms = { location["box_link"] }; checker.setCollisionObjectsTransform(names, transforms); checker.contactTest(result, test_type); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); } @@ -207,7 +206,6 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t EXPECT_EQ(checker.getCollisionMarginData().getMaxCollisionMargin(), 1.7); EXPECT_NEAR(checker.getCollisionMarginData().getPairCollisionMargin("box_link", "second_box_link"), 0.1, 1e-5); location["box_link"].translation() = Eigen::Vector3d(1.60, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -216,7 +214,7 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t tesseract_common::VectorIsometry3d transforms = { location["box_link"] }; checker.setCollisionObjectsTransform(names, transforms); checker.contactTest(result, test_type); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(result_vector.empty()); } @@ -224,7 +222,6 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t // Test object inside the contact distance only for this link pair ///////////////////////////////////////////// { - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -235,7 +232,7 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t EXPECT_NEAR(checker.getCollisionMarginData().getPairCollisionMargin("box_link", "second_box_link"), 0.25, 1e-5); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5); checker.contactTest(result, ContactRequest(test_type)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.1, 0.001); @@ -266,14 +263,13 @@ inline void runTestTyped(DiscreteContactManager& checker, ContactTestType test_t // Test object inside the contact distance ///////////////////////////////////////////// { - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.25)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.25, 1e-5); checker.contactTest(result, ContactRequest(test_type)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.1, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp index fffd49058a7..cc6aa594d54 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_capsule_unit.hpp @@ -132,7 +132,7 @@ inline void runTest(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.55, 0.0001); @@ -163,27 +163,25 @@ inline void runTest(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["capsule_link"].translation() = Eigen::Vector3d(0, 0, 1); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform(location); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.251)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.125, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp index 2135924c6a8..0d3319b99b4 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cone_unit.hpp @@ -137,7 +137,7 @@ inline void runTest(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.55, 0.0001); @@ -174,28 +174,26 @@ inline void runTest(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["cone_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); // Use different method for setting transforms checker.setCollisionObjectsTransform("cone_link", location["cone_link"]); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.251)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.25, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp index c7c87427f16..85c87bd1738 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_cylinder_unit.hpp @@ -133,7 +133,7 @@ inline void runTest(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.55, 0.0001); @@ -164,7 +164,6 @@ inline void runTest(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["cylinder_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -173,21 +172,20 @@ inline void runTest(DiscreteContactManager& checker) tesseract_common::VectorIsometry3d transforms = { location["cylinder_link"] }; checker.setCollisionObjectsTransform(names, transforms); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.251)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.25, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp index bce18e74141..08a06e0a50b 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_box_sphere_unit.hpp @@ -147,7 +147,7 @@ inline void runTestPrimitive(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.55, 0.001); @@ -182,28 +182,26 @@ inline void runTestPrimitive(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["sphere_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); // Use different method for setting transforms checker.setCollisionObjectsTransform("sphere_link", location["sphere_link"]); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.251)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.251, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.25, 0.001); @@ -262,7 +260,7 @@ inline void runTestConvex(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.53776, 0.001); @@ -292,27 +290,25 @@ inline void runTestConvex(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["sphere_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform(location); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.27)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.27, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.26224, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_clone_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_clone_unit.hpp index f5c591ebb6d..12bcdf3af13 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_clone_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_clone_unit.hpp @@ -147,7 +147,7 @@ runTest(DiscreteContactManager& checker, double dist_tol = 0.001, double nearest checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); std::vector idx = { 0, 1, 1 }; if (result_vector[0].link_names[0] != "sphere_link") @@ -160,7 +160,7 @@ runTest(DiscreteContactManager& checker, double dist_tol = 0.001, double nearest cloned_checker->contactTest(cloned_result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector cloned_result_vector; - flattenMoveResults(std::move(cloned_result), cloned_result_vector); + cloned_result.flattenMoveResults(cloned_result_vector); std::vector cloned_idx = { 0, 1, 1 }; if (cloned_result_vector[0].link_names[0] != "sphere_link") diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp index 7aed9aeffac..3d61a8e44fc 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_compound_compound_unit.hpp @@ -95,7 +95,7 @@ inline void runTestCompound(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::ALL)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); for (const auto& cr : result_vector) @@ -140,7 +140,7 @@ inline void runTestCompound(ContinuousContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::ALL)); ContactResultVector result_vector; - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); for (const auto& cr : result_vector) diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp index f762280f230..92eceb404bb 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_large_dataset_unit.hpp @@ -75,16 +75,17 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.1, 1e-5); checker.setCollisionObjectsTransform(location); + ContactResultMap result; ContactResultVector result_vector; auto start_time = std::chrono::high_resolution_clock::now(); num_interations = 1; for (auto i = 0; i < num_interations; ++i) { - ContactResultMap result; + result.clear(); result_vector.clear(); checker.contactTest(result, ContactRequest(ContactTestType::ALL)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); if (result_vector.size() != 300) for (const auto& result : result_vector) diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp index 8260098fb4d..282ccb3cdaf 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_mesh_mesh_unit.hpp @@ -146,7 +146,7 @@ inline void runTest(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::ALL)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(result_vector.size() >= 37); @@ -154,27 +154,25 @@ inline void runTest(DiscreteContactManager& checker) // Test object is out side the contact distance /////////////////////////////////////////////// location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform(location); checker.contactTest(result, ContactRequest(ContactTestType::ALL)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////////////////////////////////// // Test object inside the contact distance (Closest Feature Edge to Edge) ///////////////////////////////////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.55)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.52448, 0.001); @@ -194,7 +192,6 @@ inline void runTest(DiscreteContactManager& checker) // Test object inside the contact distance (Closest Feature Vertex to Vertex) ///////////////////////////////////////////////////////////////////////////// location["sphere1_link"].translation() = Eigen::Vector3d(0, 1, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -204,7 +201,7 @@ inline void runTest(DiscreteContactManager& checker) checker.setCollisionMarginData(CollisionMarginData(0.55)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.5, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp index ffcc14f48fc..7f25f27fa27 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_multi_threaded_unit.hpp @@ -124,7 +124,7 @@ inline void runTest(DiscreteContactManager& checker, bool use_convex_mesh = fals ContactResultMap result; manager->contactTest(result, ContactRequest(ContactTestType::ALL)); - flattenMoveResults(std::move(result), result_vector[static_cast(tn)]); + result.flattenMoveResults(result_vector[static_cast(tn)]); } auto end_time = std::chrono::high_resolution_clock::now(); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp index 8c13b1114e6..49cd324fd62 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_mesh_unit.hpp @@ -98,7 +98,7 @@ inline void runTest(DiscreteContactManager& checker, const std::string& file_pat checker.contactTest(result, ContactRequest(ContactTestType::ALL)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); const tesseract_collision::CollisionShapesConst& geom = checker.getCollisionObjectGeometries("plane_link"); const auto& mesh = std::static_pointer_cast(geom.at(0)); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp index 29a18b02b84..37feda3b200 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_octomap_unit.hpp @@ -96,7 +96,7 @@ inline void runTestOctomap(DiscreteContactManager& checker, ContactTestType test checker.contactTest(result, ContactRequest(test_type)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); for (const auto& cr : result_vector) @@ -138,7 +138,7 @@ inline void runTestOctomap(ContinuousContactManager& checker, ContactTestType te checker.contactTest(result, ContactRequest(test_type)); ContactResultVector result_vector; - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); for (const auto& cr : result_vector) diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp index a1476b46e7b..94a73834161 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_octomap_sphere_unit.hpp @@ -106,7 +106,7 @@ inline void runTestTyped(DiscreteContactManager& checker, double tol, ContactTes checker.contactTest(result, ContactRequest(test_type)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); if (test_type == ContactTestType::CLOSEST) diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp index 0d584bfe04f..494a8f51f69 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_cast_unit.hpp @@ -174,7 +174,7 @@ inline void runTestPrimitive(ContinuousContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001); @@ -241,11 +241,11 @@ inline void runTestPrimitive(ContinuousContactManager& checker) checker.setCollisionObjectsTransform(location_start, location_end); // Perform collision check - result = ContactResultMap(); + result.clear(); + result_vector.clear(); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - result_vector = ContactResultVector(); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001); @@ -330,7 +330,7 @@ inline void runTestConvex(ContinuousContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001); @@ -397,11 +397,11 @@ inline void runTestConvex(ContinuousContactManager& checker) checker.setCollisionObjectsTransform(location_start, location_end); // Perform collision check - result = ContactResultMap(); + result.clear(); + result_vector.clear(); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - result_vector = ContactResultVector(); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.0754, 0.001); diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp index 6601bba52a4..c80abf49e56 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/collision_sphere_sphere_unit.hpp @@ -149,7 +149,7 @@ inline void runTestPrimitive(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001); @@ -187,27 +187,25 @@ inline void runTestPrimitive(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionMarginData(CollisionMarginData(0.52)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001); @@ -268,7 +266,7 @@ inline void runTestConvex1(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); // Bullet: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368} // FCL: -0.270548 {0.232874,0,-0.025368} {-0.032874,0,0.025368} @@ -306,13 +304,12 @@ inline void runTestConvex1(DiscreteContactManager& checker) // Test object is out side the contact distance /////////////////////////////////////////////// location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform(location); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(result_vector.empty()); } @@ -341,7 +338,7 @@ inline void runTestConvex2(DiscreteContactManager& checker) checker.setCollisionMarginData(CollisionMarginData(0.55)); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.55, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); // Bullet: 0.524565 {0.237717,0,0} {0.7622825,0,0} Using blender this appears to be the correct result // FCL: 0.546834 {0.237717,-0.0772317,0} {0.7622825,0.0772317} @@ -394,7 +391,7 @@ inline void runTestConvex3(DiscreteContactManager& checker) ContactResultMap result; ContactResultVector result_vector; checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); // Bullet: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040} // FCL: -0.280223 {0.0425563,0.2308753,-0.0263040} {-0.0425563, -0.0308753, 0.0263040} diff --git a/tesseract_collision/core/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp b/tesseract_collision/core/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp index 9dcfd0c3e45..92f3cfccd3c 100644 --- a/tesseract_collision/core/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp +++ b/tesseract_collision/core/include/tesseract_collision/test_suite/contact_manager_config_unit.hpp @@ -157,7 +157,7 @@ inline void runTest(DiscreteContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.30, 0.0001); @@ -182,20 +182,18 @@ inline void runTest(DiscreteContactManager& checker) // Test object is out side the contact distance //////////////////////////////////////////////// location["sphere1_link"].translation() = Eigen::Vector3d(1, 0, 0); - result = ContactResultMap(); result.clear(); result_vector.clear(); checker.setCollisionObjectsTransform("sphere1_link", location["sphere1_link"]); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(result_vector.empty()); ///////////////////////////////////////////// // Test object inside the contact distance ///////////////////////////////////////////// - result = ContactResultMap(); result.clear(); result_vector.clear(); @@ -203,7 +201,7 @@ inline void runTest(DiscreteContactManager& checker) checker.applyContactManagerConfig(config); EXPECT_NEAR(checker.getCollisionMarginData().getMaxCollisionMargin(), 0.52, 1e-5); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, 0.5, 0.0001); @@ -282,7 +280,7 @@ inline void runTest(ContinuousContactManager& checker) checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); ContactResultVector result_vector; - flattenCopyResults(result, result_vector); + result.flattenCopyResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001); @@ -349,11 +347,11 @@ inline void runTest(ContinuousContactManager& checker) checker.setCollisionObjectsTransform(location_start, location_end); // Perform collision check - result = ContactResultMap(); + result.clear(); + result_vector.clear(); checker.contactTest(result, ContactRequest(ContactTestType::CLOSEST)); - result_vector = ContactResultVector(); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); EXPECT_TRUE(!result_vector.empty()); EXPECT_NEAR(result_vector[0].distance, -0.1, 0.0001); diff --git a/tesseract_collision/core/src/common.cpp b/tesseract_collision/core/src/common.cpp index 0265edb0293..9758d83b814 100644 --- a/tesseract_collision/core/src/common.cpp +++ b/tesseract_collision/core/src/common.cpp @@ -36,15 +36,11 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include namespace tesseract_collision { -ObjectPairKey getObjectPairKey(const std::string& obj1, const std::string& obj2) -{ - return obj1 < obj2 ? std::make_pair(obj1, obj2) : std::make_pair(obj2, obj1); -} - std::vector getCollisionObjectPairs(const std::vector& active_links, const std::vector& static_links, const IsContactAllowedFn& acm) @@ -63,7 +59,7 @@ std::vector getCollisionObjectPairs(const std::vector getCollisionObjectPairs(const std::vectorinsert(std::make_pair(key, data)).first->second.back()); + return &(cdata.res->addContactResult(key, contact)); } assert(cdata.req.type != ContactTestType::FIRST); - ContactResultVector& dr = (*cdata.res)[key]; if (cdata.req.type == ContactTestType::ALL) - { - dr.emplace_back(contact); - return &(dr.back()); - } + return &(cdata.res->addContactResult(key, contact)); if (cdata.req.type == ContactTestType::CLOSEST) { - if (contact.distance < dr[0].distance) - { - dr[0] = contact; - return dr.data(); - } + const auto& cv = cdata.res->at(key); + assert(!cv.empty()); + + if (contact.distance < cv.front().distance) + return &(cdata.res->setContactResult(key, contact)); } + // else if (cdata.cdata.condition == DistanceRequestType::LIMITED) // { // assert(dr.size() < cdata.req->max_contacts_per_body); diff --git a/tesseract_collision/core/src/serialization.cpp b/tesseract_collision/core/src/serialization.cpp index 8b657696f99..bf113a8af58 100644 --- a/tesseract_collision/core/src/serialization.cpp +++ b/tesseract_collision/core/src/serialization.cpp @@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -86,7 +87,32 @@ void serialize(Archive& ar, tesseract_collision::ContactResult& g, const unsigne split_free(ar, g, version); } +/***************************************************/ +/****** tesseract_collision::ContactResultMap ******/ +/***************************************************/ +template +void save(Archive& ar, const tesseract_collision::ContactResultMap& g, const unsigned int /*version*/) +{ + ar& boost::serialization::make_nvp("container", g.getContainer()); +} + +template +void load(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int /*version*/) +{ + tesseract_collision::ContactResultMap container; + ar& boost::serialization::make_nvp("container", container); + + for (const auto& c : container) + g.addContactResult(c.first, c.second); +} + +template +void serialize(Archive& ar, tesseract_collision::ContactResultMap& g, const unsigned int version) +{ + split_free(ar, g, version); +} } // namespace boost::serialization #include TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResult) +TESSERACT_SERIALIZE_SAVE_LOAD_FREE_ARCHIVES_INSTANTIATE(tesseract_collision::ContactResultMap) diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 8f590e259aa..a1731b22cde 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -57,57 +57,214 @@ void ContactResult::clear() ContactRequest::ContactRequest(ContactTestType type) : type(type) {} -std::size_t flattenMoveResults(ContactResultMap&& m, ContactResultVector& v) +ContactResult& ContactResultMap::addContactResult(const KeyType& key, ContactResult result) { - v.clear(); - v.reserve(m.size()); - for (const auto& mv : m) + ++cnt_; + auto& cv = data_[key]; + if (cv.capacity() == 0) + cv.reserve(100); + + return cv.emplace_back(std::move(result)); +} + +ContactResult& ContactResultMap::addContactResult(const KeyType& key, const MappedType& results) +{ + assert(!results.empty()); + cnt_ += static_cast(results.size()); + auto& cv = data_[key]; + cv.reserve(std::max(static_cast(100), cv.size() + results.size())); + cv.insert(cv.end(), results.begin(), results.end()); + return cv.back(); +} + +ContactResult& ContactResultMap::setContactResult(const KeyType& key, ContactResult result) +{ + auto& cv = data_[key]; + cnt_ += (1 - static_cast(cv.size())); + assert(cnt_ >= 0); + cv.clear(); + if (cv.capacity() == 0) + cv.reserve(100); + + return cv.emplace_back(std::move(result)); +} + +ContactResult& ContactResultMap::setContactResult(const KeyType& key, const MappedType& results) +{ + assert(!results.empty()); + auto& cv = data_[key]; + cnt_ += (static_cast(results.size()) - static_cast(cv.size())); + assert(cnt_ >= 0); + cv.clear(); + cv.reserve(std::max(static_cast(100), cv.size() + results.size())); + cv.insert(cv.end(), results.begin(), results.end()); + return cv.back(); +} + +void ContactResultMap::addInterpolatedCollisionResults(ContactResultMap& sub_segment_results, + int sub_segment_index, + int sub_segment_last_index, + const std::vector& active_link_names, + double segment_dt, + bool discrete, + const tesseract_collision::ContactResultMap::FilterFn& filter) +{ + // double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + for (auto& pair : sub_segment_results.data_) { - v.reserve(v.size() + mv.second.size()); - std::move(mv.second.begin(), mv.second.end(), std::back_inserter(v)); - } + // Update cc_time and cc_type + for (auto& r : pair.second) + { + // Iterate over the two time values in r.cc_time + for (size_t j = 0; j < 2; ++j) + { + if (std::find(active_link_names.begin(), active_link_names.end(), r.link_names[j]) != active_link_names.end()) + { + r.cc_time[j] = (r.cc_time[j] < 0) ? + (static_cast(sub_segment_index) * segment_dt) : + (static_cast(sub_segment_index) * segment_dt) + (r.cc_time[j] * segment_dt); + assert(r.cc_time[j] >= 0.0 && r.cc_time[j] <= 1.0); + if (sub_segment_index == 0 && + (r.cc_type[j] == tesseract_collision::ContinuousCollisionType::CCType_Time0 || discrete)) + r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Time0; + else if (sub_segment_index == sub_segment_last_index && + (r.cc_type[j] == tesseract_collision::ContinuousCollisionType::CCType_Time1 || discrete)) + r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Time1; + else + r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Between; + + // If discrete set cc_transform for discrete continuous + if (discrete) + r.cc_transform = r.transform; + } + } + } + + // Filter results + if (filter != nullptr) + filter(pair); - return v.size(); + if (!pair.second.empty()) + { + // Add results to the full segment results + cnt_ += static_cast(pair.second.size()); + auto it = data_.find(pair.first); + if (it == data_.end()) + { + data_.insert(pair); + } + else + { + assert(it != data_.end()); + // Note: Must include all contacts throughout the trajectory so the optimizer has all the information + // to understand how to adjust the start and end state to move it out of collision. Originally tried + // keeping the worst case only but ran into edge cases where this does not work in the units tests. + + it->second.reserve(it->second.size() + pair.second.size()); + it->second.insert(it->second.end(), pair.second.begin(), pair.second.end()); + } + } + } } -std::size_t flattenCopyResults(const ContactResultMap& m, ContactResultVector& v) +long ContactResultMap::count() const { return cnt_; } + +std::size_t ContactResultMap::size() const { - v.clear(); - v.reserve(m.size()); - for (const auto& mv : m) + if (cnt_ == 0) + return 0; + + std::size_t cnt{ 0 }; + for (const auto& pair : data_) { - v.reserve(v.size() + mv.second.size()); - std::copy(mv.second.begin(), mv.second.end(), std::back_inserter(v)); + if (!pair.second.empty()) + ++cnt; } - return v.size(); + return cnt; +} + +bool ContactResultMap::empty() const { return (cnt_ == 0); } + +void ContactResultMap::clear() +{ + if (cnt_ == 0) + return; + + // Only clear the vectors so the capacity stays the same + for (auto& cv : data_) + cv.second.clear(); + + cnt_ = 0; } -std::size_t flattenWrapperResults(ContactResultMap& m, std::vector>& v) +void ContactResultMap::release() +{ + data_.clear(); + cnt_ = 0; +} + +const ContactResultMap::ContainerType& ContactResultMap::getContainer() const { return data_; } + +ContactResultMap::ConstIteratorType ContactResultMap::begin() const { return data_.begin(); } + +ContactResultMap::ConstIteratorType ContactResultMap::end() const { return data_.end(); } + +ContactResultMap::ConstIteratorType ContactResultMap::cbegin() const { return data_.cbegin(); } + +ContactResultMap::ConstIteratorType ContactResultMap::cend() const { return data_.cend(); } + +const ContactResultVector& ContactResultMap::at(const KeyType& key) const { return data_.at(key); } + +ContactResultMap::ConstIteratorType ContactResultMap::find(const KeyType& key) const { return data_.find(key); } + +void ContactResultMap::flattenMoveResults(ContactResultVector& v) { v.clear(); - v.reserve(m.size()); - for (auto& mv : m) + v.reserve(static_cast(cnt_)); + for (auto& mv : data_) { - v.reserve(v.size() + mv.second.size()); - v.insert(v.end(), mv.second.begin(), mv.second.end()); + std::move(mv.second.begin(), mv.second.end(), std::back_inserter(v)); + mv.second.clear(); } + cnt_ = 0; +} - return v.size(); +void ContactResultMap::flattenCopyResults(ContactResultVector& v) const +{ + v.clear(); + v.reserve(static_cast(cnt_)); + for (const auto& mv : data_) + std::copy(mv.second.begin(), mv.second.end(), std::back_inserter(v)); } -std::size_t flattenWrapperResults(const ContactResultMap& m, - std::vector>& v) +void ContactResultMap::flattenWrapperResults(std::vector>& v) { v.clear(); - v.reserve(m.size()); - for (const auto& mv : m) - { - v.reserve(v.size() + mv.second.size()); + v.reserve(static_cast(cnt_)); + for (auto& mv : data_) v.insert(v.end(), mv.second.begin(), mv.second.end()); - } +} + +void ContactResultMap::flattenWrapperResults(std::vector>& v) const +{ + v.clear(); + v.reserve(static_cast(cnt_)); + for (const auto& mv : data_) + v.insert(v.end(), mv.second.begin(), mv.second.end()); +} - return v.size(); +void ContactResultMap::filter(const FilterFn& filter) +{ + std::size_t removed_cnt{ 0 }; + for (auto& pair : data_) + { + std::size_t current_cnt = pair.second.size(); + filter(pair); + removed_cnt += (current_cnt - pair.second.size()); + } + cnt_ -= static_cast(removed_cnt); + assert(cnt_ >= 0); } ContactTestData::ContactTestData(const std::vector& active, diff --git a/tesseract_collision/examples/box_box_example.cpp b/tesseract_collision/examples/box_box_example.cpp index 2d481f2b390..5080ea2b55b 100644 --- a/tesseract_collision/examples/box_box_example.cpp +++ b/tesseract_collision/examples/box_box_example.cpp @@ -105,7 +105,7 @@ int main(int /*argc*/, char** /*argv*/) checker.contactTest(result, request); ContactResultVector result_vector; - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str()); CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance); @@ -128,13 +128,12 @@ int main(int /*argc*/, char** /*argv*/) // documentation:end:10: Set collision object transform // documentation:start:11: Perform collision check - result = ContactResultMap(); result.clear(); result_vector.clear(); // Check for collision after moving object checker.contactTest(result, request); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str()); // documentation:end:11: Perform collision check @@ -145,13 +144,12 @@ int main(int /*argc*/, char** /*argv*/) // documentation:start:13: Perform collision check CONSOLE_BRIDGE_logInform("Test object inside the contact distance"); - result = ContactResultMap(); result.clear(); result_vector.clear(); // Check for contact with new threshold checker.contactTest(result, request); - flattenMoveResults(std::move(result), result_vector); + result.flattenMoveResults(result_vector); CONSOLE_BRIDGE_logInform("Has collision: %s", toString(result_vector.empty()).c_str()); CONSOLE_BRIDGE_logInform("Distance: %f", result_vector[0].distance); diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index 721599a6e05..1ad708f0db5 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -257,9 +257,9 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi contact.distance = -1.0 * fcl_contact.penetration_depth; contact.normal = fcl_contact.normal; - ObjectPairKey pc = getObjectPairKey(cd1->getName(), cd2->getName()); - const auto& it = cdata->res->find(pc); - bool found = (it != cdata->res->end()); + ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd1->getName(), cd2->getName()); + const auto it = cdata->res->find(pc); + bool found = (it != cdata->res->end() && !it->second.empty()); processResult(*cdata, contact, pc, found); } @@ -322,9 +322,9 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void // TODO: There is an issue with FCL need to track down assert(!std::isnan(contact.nearest_points[0](0))); - ObjectPairKey pc = getObjectPairKey(cd1->getName(), cd2->getName()); - const auto& it = cdata->res->find(pc); - bool found = (it != cdata->res->end()); + ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd1->getName(), cd2->getName()); + const auto it = cdata->res->find(pc); + bool found = (it != cdata->res->end() && !it->second.empty()); processResult(*cdata, contact, pc, found); } diff --git a/tesseract_collision/test/benchmarks/collision_profile.cpp b/tesseract_collision/test/benchmarks/collision_profile.cpp index 55fec54b87c..3889601db62 100644 --- a/tesseract_collision/test/benchmarks/collision_profile.cpp +++ b/tesseract_collision/test/benchmarks/collision_profile.cpp @@ -176,10 +176,11 @@ void runDiscreteProfile(bool use_single_link, bool use_convex_mesh, double conta std::vector poses = getTransforms(50); std::vector checkers = { bt_simple_checker, bt_bvh_checker, fcl_bvh_checker }; std::vector checker_names = { "BtSimple", "BtBVH", "FCLBVH" }; - std::vector checker_contacts = { 0, 0, 0 }; + std::vector checker_contacts = { 0, 0, 0 }; std::printf("Total number of shape: %d\n", int(DIM * DIM * DIM)); // for (std::size_t i = 0; i < checkers.size(); ++i) + ContactResultMap result; for (std::size_t i = 0; i < 2; ++i) { addCollisionObjects(*checkers[i], use_single_link, use_convex_mesh); @@ -191,9 +192,9 @@ void runDiscreteProfile(bool use_single_link, bool use_convex_mesh, double conta checkers[i]->setCollisionObjectsTransform("move_link", pose); // Perform collision check - ContactResultMap result; + result.clear(); checkers[i]->contactTest(result, ContactTestType::FIRST); - checker_contacts[i] += result.size(); + checker_contacts[i] += result.count(); } auto end_time = std::chrono::high_resolution_clock::now(); double total_time = std::chrono::duration(end_time - start_time).count(); @@ -211,7 +212,7 @@ void runContinuousProfile(bool use_single_link, bool use_convex_mesh, double con std::vector poses = getTransforms(50); std::vector checkers = { bt_simple_checker, bt_bvh_checker }; std::vector checker_names = { "BtCastSimple", "BtCastBVH" }; - std::vector checker_contacts = { 0, 0, 0 }; + std::vector checker_contacts = { 0, 0, 0 }; Eigen::Isometry3d delta_pose; delta_pose.setIdentity(); @@ -219,6 +220,7 @@ void runContinuousProfile(bool use_single_link, bool use_convex_mesh, double con std::printf("Total number of shape: %d\n", int(DIM * DIM * DIM)); // for (std::size_t i = 0; i < checkers.size(); ++i) + ContactResultMap result; for (std::size_t i = 0; i < 2; ++i) { addCollisionObjects(*checkers[i], use_single_link, use_convex_mesh); @@ -230,9 +232,9 @@ void runContinuousProfile(bool use_single_link, bool use_convex_mesh, double con checkers[i]->setCollisionObjectsTransform("move_link", pose, pose * delta_pose); // Perform collision check - ContactResultMap result; + result.clear(); checkers[i]->contactTest(result, ContactTestType::FIRST); - checker_contacts[i] += result.size(); + checker_contacts[i] += result.count(); } auto end_time = std::chrono::high_resolution_clock::now(); double total_time = std::chrono::duration(end_time - start_time).count(); diff --git a/tesseract_collision/test/collision_core_unit.cpp b/tesseract_collision/test/collision_core_unit.cpp index fcc89b3401e..22a5b423fbd 100644 --- a/tesseract_collision/test/collision_core_unit.cpp +++ b/tesseract_collision/test/collision_core_unit.cpp @@ -14,15 +14,15 @@ TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT std::vector static_links{ "base_link", "part_link" }; std::vector check_pairs; - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_1", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_1", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_2", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("base_link", "link_1")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("base_link", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("base_link", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_1")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_2", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_1")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_1")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_3")); std::vector pairs = tesseract_collision::getCollisionObjectPairs(active_links, static_links); @@ -31,19 +31,19 @@ TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT // Now check provided a is contact allowed function auto acm = [](const std::string& s1, const std::string& s2) { - return (tesseract_collision::getObjectPairKey("base_link", "link_1") == - tesseract_collision::getObjectPairKey(s1, s2)); + return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") == + tesseract_common::makeOrderedLinkPair(s1, s2)); }; check_pairs.clear(); - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_1", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_1", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("link_2", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("base_link", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("base_link", "link_3")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_1")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_2")); - check_pairs.push_back(tesseract_collision::getObjectPairKey("part_link", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_1", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("link_2", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("base_link", "link_3")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_1")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_2")); + check_pairs.push_back(tesseract_common::makeOrderedLinkPair("part_link", "link_3")); pairs = tesseract_collision::getCollisionObjectPairs(active_links, static_links, acm); @@ -53,8 +53,8 @@ TEST(TesseractCoreUnit, getCollisionObjectPairsUnit) // NOLINT TEST(TesseractCoreUnit, isContactAllowedUnit) // NOLINT { auto acm = [](const std::string& s1, const std::string& s2) { - return (tesseract_collision::getObjectPairKey("base_link", "link_1") == - tesseract_collision::getObjectPairKey(s1, s2)); + return (tesseract_common::makeOrderedLinkPair("base_link", "link_1") == + tesseract_common::makeOrderedLinkPair(s1, s2)); }; EXPECT_TRUE(tesseract_collision::isContactAllowed("base_link", "base_link", acm, false)); @@ -187,6 +187,368 @@ TEST(TesseractCoreUnit, ContactResultsUnit) // NOLINT EXPECT_EQ(results.single_contact_point, false); } +TEST(TesseractCoreUnit, ContactResultMapUnit) // NOLINT +{ + { // Test construction state + tesseract_collision::ContactResultMap result_map; + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_EQ(result_map.getContainer().size(), 0); + } + + auto key1 = tesseract_common::makeOrderedLinkPair("link1", "link2"); + auto key2 = tesseract_common::makeOrderedLinkPair("link2", "link3"); + + { // Test addContactResult single method + tesseract_collision::ContactResultMap result_map; + result_map.addContactResult(key1, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 1); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + result_map.addContactResult(key1, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + result_map.addContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + // test clear + result_map.clear(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + // test release + result_map.release(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_TRUE(result_map.getContainer().empty()); + } + + { // Test addContactResult vector method + tesseract_collision::ContactResultMap result_map; + result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 4); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 4); + + result_map.addContactResult(key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 6); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 4); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + // test release + result_map.release(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_TRUE(result_map.getContainer().empty()); + } + + { // Test setContactResult single method + tesseract_collision::ContactResultMap result_map; + result_map.setContactResult(key1, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 1); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + result_map.addContactResult(key1, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + result_map.setContactResult(key1, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 1); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + result_map.setContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + // test clear + result_map.clear(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + // test release + result_map.release(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_TRUE(result_map.getContainer().empty()); + } + + { // Test setContactResult vector method + tesseract_collision::ContactResultMap result_map; + result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 4); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 4); + + result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 2); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 1); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + result_map.setContactResult(key2, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + EXPECT_EQ(result_map.count(), 4); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + // test release + result_map.release(); + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_TRUE(result_map.getContainer().empty()); + } + + { // flatten move + tesseract_collision::ContactResultMap result_map; + result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + result_map.addContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + tesseract_collision::ContactResultVector result_vector; + result_map.flattenMoveResults(result_vector); + EXPECT_EQ(result_vector.size(), 3); + + EXPECT_EQ(result_map.count(), 0); + EXPECT_EQ(result_map.size(), 0); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + } + + { // flatten copy + tesseract_collision::ContactResultMap result_map; + result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + result_map.setContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + tesseract_collision::ContactResultVector result_vector; + result_map.flattenCopyResults(result_vector); + EXPECT_EQ(result_vector.size(), 3); + + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + } + + { // flatten reference wrapper + tesseract_collision::ContactResultMap result_map; + result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + result_map.addContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + std::vector> result_vector; + result_map.flattenWrapperResults(result_vector); + EXPECT_EQ(result_vector.size(), 3); + + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + } + + { // flatten reference wrapper const + tesseract_collision::ContactResultMap result_map; + result_map.addContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + result_map.setContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + std::vector> result_vector; + result_map.flattenWrapperResults(result_vector); + EXPECT_EQ(result_vector.size(), 3); + + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + } + + { // filter + tesseract_collision::ContactResultMap result_map; + result_map.setContactResult(key1, { tesseract_collision::ContactResult{}, tesseract_collision::ContactResult{} }); + result_map.addContactResult(key2, tesseract_collision::ContactResult{}); + EXPECT_EQ(result_map.count(), 3); + EXPECT_EQ(result_map.size(), 2); + EXPECT_EQ(result_map.getContainer().size(), 2); + auto it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 2); + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + + auto filter = [key1](tesseract_collision::ContactResultMap::PairType& pair) { + if (key1 == pair.first) + pair.second.clear(); + }; + result_map.filter(filter); + + EXPECT_EQ(result_map.count(), 1); + EXPECT_EQ(result_map.size(), 1); + EXPECT_EQ(result_map.getContainer().size(), 2); + it = result_map.find(key1); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 0); + EXPECT_TRUE(it->second.capacity() > 0); + + it = result_map.find(key2); + EXPECT_TRUE(it != result_map.end()); + EXPECT_EQ(it->second.size(), 1); + } +} + TEST(TesseractCoreUnit, CollisionCheckConfigUnit) // NOLINT { tesseract_collision::ContactRequest request; diff --git a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h index e5c5eb49add..2c2e5f3ff72 100644 --- a/tesseract_common/include/tesseract_common/allowed_collision_matrix.h +++ b/tesseract_common/include/tesseract_common/allowed_collision_matrix.h @@ -96,7 +96,8 @@ class AllowedCollisionMatrix */ virtual bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const { - auto link_pair = tesseract_common::makeOrderedLinkPair(link_name1, link_name2); + thread_local LinkNamesPair link_pair; + tesseract_common::makeOrderedLinkPair(link_pair, link_name1, link_name2); return (lookup_table_.find(link_pair) != lookup_table_.end()); } diff --git a/tesseract_common/include/tesseract_common/collision_margin_data.h b/tesseract_common/include/tesseract_common/collision_margin_data.h index 4297d148a3a..6919ce5b083 100644 --- a/tesseract_common/include/tesseract_common/collision_margin_data.h +++ b/tesseract_common/include/tesseract_common/collision_margin_data.h @@ -109,7 +109,7 @@ class CollisionMarginData * @brief Get the default collision margin * @return default collision margin */ - double getDefaultCollisionMargin() const { return default_collision_margin_; }; + double getDefaultCollisionMargin() const { return default_collision_margin_; } /** * @brief Set the margin for a given contact pair @@ -139,7 +139,8 @@ class CollisionMarginData */ double getPairCollisionMargin(const std::string& obj1, const std::string& obj2) const { - auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); + thread_local LinkNamesPair key; + tesseract_common::makeOrderedLinkPair(key, obj1, obj2); const auto it = lookup_table_.find(key); if (it != lookup_table_.end()) diff --git a/tesseract_common/include/tesseract_common/types.h b/tesseract_common/include/tesseract_common/types.h index c91da51d76e..bb4827ce5e7 100644 --- a/tesseract_common/include/tesseract_common/types.h +++ b/tesseract_common/include/tesseract_common/types.h @@ -84,6 +84,7 @@ struct PairHash * @return LinkNamesPair a lexicographically sorted pair of strings */ LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::string& link_name2); +void makeOrderedLinkPair(LinkNamesPair& pair, const std::string& link_name1, const std::string& link_name2); /** @brief The Plugin Information struct */ // NOLINTNEXTLINE diff --git a/tesseract_common/src/types.cpp b/tesseract_common/src/types.cpp index 8d0caeefd40..6fa4026ba6a 100644 --- a/tesseract_common/src/types.cpp +++ b/tesseract_common/src/types.cpp @@ -46,15 +46,28 @@ namespace tesseract_common { std::size_t PairHash::operator()(const LinkNamesPair& pair) const { - return std::hash()(pair.first + pair.second); + thread_local std::string key; + key = pair.first + pair.second; + return std::hash()(key); } LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::string& link_name2) { - if (link_name1 <= link_name2) - return std::make_pair(link_name1, link_name2); + return (link_name1 <= link_name2) ? std::make_pair(link_name1, link_name2) : std::make_pair(link_name2, link_name1); +} - return std::make_pair(link_name2, link_name1); +void makeOrderedLinkPair(LinkNamesPair& pair, const std::string& link_name1, const std::string& link_name2) +{ + if (link_name1 <= link_name2) + { + pair.first = link_name1; + pair.second = link_name2; + } + else + { + pair.first = link_name2; + pair.second = link_name1; + } } /*********************************************************/ diff --git a/tesseract_environment/include/tesseract_environment/utils.h b/tesseract_environment/include/tesseract_environment/utils.h index 5c0fa77a135..211c7d0e18d 100644 --- a/tesseract_environment/include/tesseract_environment/utils.h +++ b/tesseract_environment/include/tesseract_environment/utils.h @@ -53,70 +53,56 @@ void getActiveLinkNamesRecursive(std::vector& active_links, /** * @brief Should perform a continuous collision check between two states configuring the manager with the config + * @param contact_results The contact results to populate. It does not get cleared * @param manager A continuous contact manager * @param state0 First environment state * @param state1 Second environment state * @param config CollisionCheckConfig used to specify collision check settings - * @return Return the contact results map. If empty no contacts were found */ -tesseract_collision::ContactResultMap checkTrajectorySegment(tesseract_collision::ContinuousContactManager& manager, - const tesseract_common::TransformMap& state0, - const tesseract_common::TransformMap& state1, - const tesseract_collision::CollisionCheckConfig& config); +void checkTrajectorySegment(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::ContinuousContactManager& manager, + const tesseract_common::TransformMap& state0, + const tesseract_common::TransformMap& state1, + const tesseract_collision::CollisionCheckConfig& config); /** * @brief Should perform a continuous collision check between two states only passing along the contact_request to the * manager + * @param contact_results The contact results to populate. It does not get cleared * @param manager A continuous contact manager * @param state0 First environment state * @param state1 Second environment state * @param contact_request Contact request passed to the manager - * @return Return the contact results map. If empty not contacts were found */ -tesseract_collision::ContactResultMap -checkTrajectorySegment(tesseract_collision::ContinuousContactManager& manager, - const tesseract_common::TransformMap& state0, - const tesseract_common::TransformMap& state1, - const tesseract_collision::ContactRequest& contact_request); +void checkTrajectorySegment(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::ContinuousContactManager& manager, + const tesseract_common::TransformMap& state0, + const tesseract_common::TransformMap& state1, + const tesseract_collision::ContactRequest& contact_request); /** * @brief Should perform a discrete collision check a state first configuring manager with config + * @param contact_results The contact results to populate. It does not get cleared * @param manager A discrete contact manager * @param state First environment state * @param config CollisionCheckConfig used to specify collision check settings - * @return Return the contact results map. If empty no contacts were found */ -tesseract_collision::ContactResultMap checkTrajectoryState(tesseract_collision::DiscreteContactManager& manager, - const tesseract_common::TransformMap& state, - const tesseract_collision::CollisionCheckConfig& config); +void checkTrajectoryState(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::DiscreteContactManager& manager, + const tesseract_common::TransformMap& state, + const tesseract_collision::CollisionCheckConfig& config); /** * @brief Should perform a discrete collision check a state only passing contact_request to the manager + * @param contact_results The contact results to populate. It does not get cleared * @param manager A discrete contact manager * @param state First environment state * @param contact_request Contact request passed to the manager - * @return Return the contact results map. If empty no contacts were found */ -tesseract_collision::ContactResultMap checkTrajectoryState(tesseract_collision::DiscreteContactManager& manager, - const tesseract_common::TransformMap& state, - const tesseract_collision::ContactRequest& contact_request); - -/** - * @brief This processes interpolated contact results and updated cc_time and cc_type - * @details This is copied from the trajopt utility processInterpolatedCollisionResults - * @param segment_results The full results to store sub segment results in after processing - * @param sub_segment_results The interpolated results to process - * @param sub_segment_index The current sub segment index - * @param sub_segment_last_index The last sub segment index - * @param manip_active_link_names The active link names - * @param discrete If discrete contact checker was used - */ -void processInterpolatedSubSegmentCollisionResults(tesseract_collision::ContactResultMap& segment_results, - tesseract_collision::ContactResultMap& sub_segment_results, - int sub_segment_index, - int sub_segment_last_index, - const std::vector& manip_active_link_names, - bool discrete); +void checkTrajectoryState(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::DiscreteContactManager& manager, + const tesseract_common::TransformMap& state, + const tesseract_collision::ContactRequest& contact_request); /** * @brief Should perform a continuous collision check over the trajectory and stop on first collision. diff --git a/tesseract_environment/src/utils.cpp b/tesseract_environment/src/utils.cpp index b09cd4d4836..5c5ed84c17f 100644 --- a/tesseract_environment/src/utils.cpp +++ b/tesseract_environment/src/utils.cpp @@ -63,142 +63,76 @@ void getActiveLinkNamesRecursive(std::vector& active_links, } } -tesseract_collision::ContactResultMap checkTrajectorySegment(tesseract_collision::ContinuousContactManager& manager, - const tesseract_common::TransformMap& state0, - const tesseract_common::TransformMap& state1, - const tesseract_collision::CollisionCheckConfig& config) +void checkTrajectorySegment(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::ContinuousContactManager& manager, + const tesseract_common::TransformMap& state0, + const tesseract_common::TransformMap& state1, + const tesseract_collision::CollisionCheckConfig& config) { manager.applyContactManagerConfig(config.contact_manager_config); - return checkTrajectorySegment(manager, state0, state1, config.contact_request); + checkTrajectorySegment(contact_results, manager, state0, state1, config.contact_request); } -tesseract_collision::ContactResultMap checkTrajectorySegment(tesseract_collision::ContinuousContactManager& manager, - const tesseract_common::TransformMap& state0, - const tesseract_common::TransformMap& state1, - const tesseract_collision::ContactRequest& contact_request) +void checkTrajectorySegment(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::ContinuousContactManager& manager, + const tesseract_common::TransformMap& state0, + const tesseract_common::TransformMap& state1, + const tesseract_collision::ContactRequest& contact_request) { for (const auto& link_name : manager.getActiveCollisionObjects()) manager.setCollisionObjectsTransform(link_name, state0.at(link_name), state1.at(link_name)); - tesseract_collision::ContactResultMap collisions; - manager.contactTest(collisions, contact_request); + manager.contactTest(contact_results, contact_request); - if (!collisions.empty()) + if (!contact_results.empty()) { if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { - for (auto& collision : collisions) + for (const auto& collision : contact_results) { std::stringstream ss; ss << "Continuous collision detected between '" << collision.first.first << "' and '" << collision.first.second << "' with distance " << collision.second.front().distance << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } } - - return collisions; } -tesseract_collision::ContactResultMap checkTrajectoryState(tesseract_collision::DiscreteContactManager& manager, - const tesseract_common::TransformMap& state, - const tesseract_collision::CollisionCheckConfig& config) +void checkTrajectoryState(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::DiscreteContactManager& manager, + const tesseract_common::TransformMap& state, + const tesseract_collision::CollisionCheckConfig& config) { manager.applyContactManagerConfig(config.contact_manager_config); - return checkTrajectoryState(manager, state, config.contact_request); + checkTrajectoryState(contact_results, manager, state, config.contact_request); } -tesseract_collision::ContactResultMap checkTrajectoryState(tesseract_collision::DiscreteContactManager& manager, - const tesseract_common::TransformMap& state, - const tesseract_collision::ContactRequest& contact_request) +void checkTrajectoryState(tesseract_collision::ContactResultMap& contact_results, + tesseract_collision::DiscreteContactManager& manager, + const tesseract_common::TransformMap& state, + const tesseract_collision::ContactRequest& contact_request) { - tesseract_collision::ContactResultMap collisions; - for (const auto& link_name : manager.getActiveCollisionObjects()) manager.setCollisionObjectsTransform(link_name, state.at(link_name)); - manager.contactTest(collisions, contact_request); + manager.contactTest(contact_results, contact_request); - if (!collisions.empty()) + if (!contact_results.empty()) { if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { - for (auto& collision : collisions) + for (const auto& collision : contact_results) { std::stringstream ss; ss << "Discrete collision detected between '" << collision.first.first << "' and '" << collision.first.second << "' with distance " << collision.second.front().distance << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); - } - } - } - - return collisions; -} - -/** - * @brief This process contact results - * @param segment_results - * @param sub_segment_results - * @param segment_index - * @param segment_last_index - * @param manip_active_link_names - * @param discrete - */ -void processInterpolatedSubSegmentCollisionResults(tesseract_collision::ContactResultMap& segment_results, - tesseract_collision::ContactResultMap& sub_segment_results, - int sub_segment_index, - int sub_segment_last_index, - const std::vector& manip_active_link_names, - bool discrete) -{ - double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; - for (auto& pair : sub_segment_results) - { - // Update cc_time and cc_type - for (auto& r : pair.second) - { - // Iterate over the two time values in r.cc_time - for (size_t j = 0; j < 2; ++j) - { - if (std::find(manip_active_link_names.begin(), manip_active_link_names.end(), r.link_names[j]) != - manip_active_link_names.end()) - { - r.cc_time[j] = (r.cc_time[j] < 0) ? - (static_cast(sub_segment_index) * segment_dt) : - (static_cast(sub_segment_index) * segment_dt) + (r.cc_time[j] * segment_dt); - assert(r.cc_time[j] >= 0.0 && r.cc_time[j] <= 1.0); - if (sub_segment_index == 0 && - (r.cc_type[j] == tesseract_collision::ContinuousCollisionType::CCType_Time0 || discrete)) - r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Time0; - else if (sub_segment_index == sub_segment_last_index && - (r.cc_type[j] == tesseract_collision::ContinuousCollisionType::CCType_Time1 || discrete)) - r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Time1; - else - r.cc_type[j] = tesseract_collision::ContinuousCollisionType::CCType_Between; - - // If discrete set cc_transform for discrete continuous - if (discrete) - r.cc_transform = r.transform; - } - } - - if (sub_segment_last_index > 0) - { - // Add results to the full segment results - auto it = segment_results.find(pair.first); - if (it == segment_results.end()) - segment_results.insert(pair); - else - it->second.insert(it->second.end(), pair.second.begin(), pair.second.end()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } } - - if (sub_segment_last_index == 0) - segment_results = sub_segment_results; } bool checkTrajectory(std::vector& contacts, @@ -219,14 +153,19 @@ bool checkTrajectory(std::vector& contact manager.applyContactManagerConfig(config.contact_manager_config); + contacts.clear(); + contacts.reserve(static_cast(traj.rows())); + + /** @brief Making this thread_local does not help because it is not called enough during planning */ + tesseract_collision::ContactResultMap state_results; + tesseract_collision::ContactResultMap sub_state_results; + bool found = false; - contacts.resize(static_cast(traj.rows() - 1)); if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) { for (int iStep = 0; iStep < traj.rows() - 1; ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); + state_results.clear(); double dist = (traj.row(iStep + 1) - traj.row(iStep)).norm(); if (dist > config.longest_valid_segment_length) @@ -236,20 +175,23 @@ bool checkTrajectory(std::vector& contact for (long iVar = 0; iVar < traj.cols(); ++iVar) subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, traj.row(iStep)(iVar), traj.row(iStep + 1)(iVar)); + auto sub_segment_last_index = static_cast(subtraj.rows() - 1); for (int iSubStep = 0; iSubStep < subtraj.rows() - 1; ++iSubStep) { tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names, subtraj.row(iSubStep)); tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names, subtraj.row(iSubStep + 1)); - tesseract_collision::ContactResultMap sub_segment_results = - checkTrajectorySegment(manager, state0.link_transforms, state1.link_transforms, config.contact_request); - if (!sub_segment_results.empty()) + sub_state_results.clear(); + checkTrajectorySegment( + sub_state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request); + if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults(segment_results, - sub_segment_results, + double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + state_results.addInterpolatedCollisionResults(sub_state_results, iSubStep, - static_cast(subtraj.rows() - 1), + sub_segment_last_index, manager.getActiveCollisionObjects(), + segment_dt, false); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -266,13 +208,14 @@ bool checkTrajectory(std::vector& contact << " State0: " << subtraj.row(iSubStep) << std::endl << " State1: " << subtraj.row(iSubStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -281,9 +224,10 @@ bool checkTrajectory(std::vector& contact { tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names, traj.row(iStep)); tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names, traj.row(iStep + 1)); - segment_results = - checkTrajectorySegment(manager, state0.link_transforms, state1.link_transforms, config.contact_request); - if (!segment_results.empty()) + // state_results is already cleared above + checkTrajectorySegment( + state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request); + if (!state_results.empty()) { found = true; if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -299,9 +243,10 @@ bool checkTrajectory(std::vector& contact << " State0: " << traj.row(iStep) << std::endl << " State1: " << traj.row(iStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -312,15 +257,13 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows() - 1; ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); - tesseract_scene_graph::SceneState state0 = state_solver.getState(joint_names, traj.row(iStep)); tesseract_scene_graph::SceneState state1 = state_solver.getState(joint_names, traj.row(iStep + 1)); - segment_results = - checkTrajectorySegment(manager, state0.link_transforms, state1.link_transforms, config.contact_request); - if (!segment_results.empty()) + state_results.clear(); + checkTrajectorySegment( + state_results, manager, state0.link_transforms, state1.link_transforms, config.contact_request); + if (!state_results.empty()) { found = true; if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -336,9 +279,10 @@ bool checkTrajectory(std::vector& contact << " State0: " << traj.row(iStep) << std::endl << " State1: " << traj.row(iStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -365,14 +309,19 @@ bool checkTrajectory(std::vector& contact manager.applyContactManagerConfig(config.contact_manager_config); + contacts.clear(); + contacts.reserve(static_cast(traj.rows())); + + /** @brief Making this thread_local does not help because it is not called enough during planning */ + tesseract_collision::ContactResultMap state_results; + tesseract_collision::ContactResultMap sub_state_results; + bool found = false; - contacts.resize(static_cast(traj.rows() - 1)); if (config.type == tesseract_collision::CollisionEvaluatorType::LVS_CONTINUOUS) { for (int iStep = 0; iStep < traj.rows() - 1; ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); + state_results.clear(); double dist = (traj.row(iStep + 1) - traj.row(iStep)).norm(); if (dist > config.longest_valid_segment_length) @@ -382,20 +331,22 @@ bool checkTrajectory(std::vector& contact for (long iVar = 0; iVar < traj.cols(); ++iVar) subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, traj.row(iStep)(iVar), traj.row(iStep + 1)(iVar)); + auto sub_segment_last_index = static_cast(subtraj.rows() - 1); for (int iSubStep = 0; iSubStep < subtraj.rows() - 1; ++iSubStep) { tesseract_common::TransformMap state0 = manip.calcFwdKin(subtraj.row(iSubStep)); tesseract_common::TransformMap state1 = manip.calcFwdKin(subtraj.row(iSubStep + 1)); - tesseract_collision::ContactResultMap sub_segment_results = - checkTrajectorySegment(manager, state0, state1, config.contact_request); - if (!sub_segment_results.empty()) + sub_state_results.clear(); + checkTrajectorySegment(sub_state_results, manager, state0, state1, config.contact_request); + if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults(segment_results, - sub_segment_results, + double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + state_results.addInterpolatedCollisionResults(sub_state_results, iSubStep, - static_cast(subtraj.rows() - 1), + sub_segment_last_index, manager.getActiveCollisionObjects(), + segment_dt, false); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -412,13 +363,14 @@ bool checkTrajectory(std::vector& contact << " State0: " << subtraj.row(iSubStep) << std::endl << " State1: " << subtraj.row(iSubStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -427,8 +379,9 @@ bool checkTrajectory(std::vector& contact { tesseract_common::TransformMap state0 = manip.calcFwdKin(traj.row(iStep)); tesseract_common::TransformMap state1 = manip.calcFwdKin(traj.row(iStep + 1)); - segment_results = checkTrajectorySegment(manager, state0, state1, config.contact_request); - if (!segment_results.empty()) + // state_results is already cleared above + checkTrajectorySegment(state_results, manager, state0, state1, config.contact_request); + if (!state_results.empty()) { found = true; if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -444,9 +397,10 @@ bool checkTrajectory(std::vector& contact << " State0: " << traj.row(iStep) << std::endl << " State1: " << traj.row(iStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -457,14 +411,11 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows() - 1; ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); - tesseract_common::TransformMap state0 = manip.calcFwdKin(traj.row(iStep)); tesseract_common::TransformMap state1 = manip.calcFwdKin(traj.row(iStep + 1)); - - segment_results = checkTrajectorySegment(manager, state0, state1, config.contact_request); - if (!segment_results.empty()) + state_results.clear(); + checkTrajectorySegment(state_results, manager, state0, state1, config.contact_request); + if (!state_results.empty()) { found = true; if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -480,9 +431,10 @@ bool checkTrajectory(std::vector& contact << " State0: " << traj.row(iStep) << std::endl << " State1: " << traj.row(iStep + 1) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -509,20 +461,23 @@ bool checkTrajectory(std::vector& contact manager.applyContactManagerConfig(config.contact_manager_config); - contacts.resize(static_cast(traj.rows())); + contacts.clear(); + contacts.reserve(static_cast(traj.rows())); + + /** @brief Making this thread_local does not help because it is not called enough during planning */ + tesseract_collision::ContactResultMap state_results; + tesseract_collision::ContactResultMap sub_state_results; if (traj.rows() == 1) { - tesseract_collision::ContactResultMap& state_results = contacts[0]; + auto sub_segment_last_index = static_cast(traj.rows() - 1); state_results.clear(); tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, traj.row(0)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state.link_transforms, config.contact_request); - processInterpolatedSubSegmentCollisionResults(state_results, - sub_state_results, - 0, - static_cast(traj.rows() - 1), - manager.getActiveCollisionObjects(), - true); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state.link_transforms, config.contact_request); + double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, sub_segment_last_index, manager.getActiveCollisionObjects(), segment_dt, true); + contacts.push_back(state_results); return (!state_results.empty()); } @@ -531,8 +486,7 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows(); ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); + state_results.clear(); double dist = -1; if (iStep < traj.rows() - 1) @@ -545,19 +499,21 @@ bool checkTrajectory(std::vector& contact for (long iVar = 0; iVar < traj.cols(); ++iVar) subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, traj.row(iStep)(iVar), traj.row(iStep + 1)(iVar)); + auto sub_segment_last_index = static_cast(subtraj.rows() - 1); for (int iSubStep = 0; iSubStep < subtraj.rows() - 1; ++iSubStep) { tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, subtraj.row(iSubStep)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state.link_transforms, config.contact_request); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state.link_transforms, config.contact_request); if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults(segment_results, - sub_state_results, + double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + state_results.addInterpolatedCollisionResults(sub_state_results, iSubStep, - static_cast(subtraj.rows() - 1), + sub_segment_last_index, manager.getActiveCollisionObjects(), + segment_dt, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -572,13 +528,14 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State: " << subtraj.row(iSubStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -586,13 +543,13 @@ bool checkTrajectory(std::vector& contact else { tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, traj.row(iStep)); - tesseract_collision::ContactResultMap sub_segment_results = - checkTrajectoryState(manager, state.link_transforms, config.contact_request); - if (!sub_segment_results.empty()) + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state.link_transforms, config.contact_request); + if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults( - segment_results, sub_segment_results, 0, 0, manager.getActiveCollisionObjects(), true); + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { std::stringstream ss; @@ -604,9 +561,10 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State: " << traj.row(iStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -617,17 +575,16 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows(); ++iStep) { - tesseract_collision::ContactResultMap& state_results = contacts[static_cast(iStep)]; state_results.clear(); tesseract_scene_graph::SceneState state = state_solver.getState(joint_names, traj.row(iStep)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state.link_transforms, config.contact_request); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state.link_transforms, config.contact_request); if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults( - state_results, sub_state_results, 0, 0, manager.getActiveCollisionObjects(), true); + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { std::stringstream ss; @@ -639,9 +596,10 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State0: " << traj.row(iStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -666,17 +624,20 @@ bool checkTrajectory(std::vector& contact manager.applyContactManagerConfig(config.contact_manager_config); - contacts.resize(static_cast(traj.rows())); + contacts.clear(); + contacts.reserve(static_cast(traj.rows())); + + /** @brief Making this thread_local does not help because it is not called enough during planning */ + tesseract_collision::ContactResultMap state_results; + tesseract_collision::ContactResultMap sub_state_results; if (traj.rows() == 1) { - tesseract_collision::ContactResultMap& state_results = contacts[0]; - state_results.clear(); - tesseract_common::TransformMap state = manip.calcFwdKin(traj.row(0)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state, config.contact_request); - processInterpolatedSubSegmentCollisionResults( - state_results, sub_state_results, 0, 0, manager.getActiveCollisionObjects(), true); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state, config.contact_request); + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); + contacts.push_back(state_results); return (!state_results.empty()); } @@ -685,8 +646,7 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows(); ++iStep) { - tesseract_collision::ContactResultMap& segment_results = contacts[static_cast(iStep)]; - segment_results.clear(); + state_results.clear(); double dist = -1; if (iStep < traj.rows() - 1) @@ -699,19 +659,21 @@ bool checkTrajectory(std::vector& contact for (long iVar = 0; iVar < traj.cols(); ++iVar) subtraj.col(iVar) = Eigen::VectorXd::LinSpaced(cnt, traj.row(iStep)(iVar), traj.row(iStep + 1)(iVar)); + auto sub_segment_last_index = static_cast(subtraj.rows() - 1); for (int iSubStep = 0; iSubStep < subtraj.rows() - 1; ++iSubStep) { tesseract_common::TransformMap state = manip.calcFwdKin(subtraj.row(iSubStep)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state, config.contact_request); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state, config.contact_request); if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults(segment_results, - sub_state_results, + double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; + state_results.addInterpolatedCollisionResults(sub_state_results, iSubStep, - static_cast(subtraj.rows() - 1), + sub_segment_last_index, manager.getActiveCollisionObjects(), + segment_dt, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) @@ -726,13 +688,14 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State: " << subtraj.row(iSubStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -740,13 +703,13 @@ bool checkTrajectory(std::vector& contact else { tesseract_common::TransformMap state = manip.calcFwdKin(traj.row(iStep)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state, config.contact_request); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state, config.contact_request); if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults( - segment_results, sub_state_results, 0, 0, manager.getActiveCollisionObjects(), true); + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { @@ -759,9 +722,10 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State: " << traj.row(iStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; @@ -772,17 +736,16 @@ bool checkTrajectory(std::vector& contact { for (int iStep = 0; iStep < traj.rows(); ++iStep) { - tesseract_collision::ContactResultMap& state_results = contacts[static_cast(iStep)]; state_results.clear(); tesseract_common::TransformMap state = manip.calcFwdKin(traj.row(iStep)); - tesseract_collision::ContactResultMap sub_state_results = - checkTrajectoryState(manager, state, config.contact_request); + sub_state_results.clear(); + checkTrajectoryState(sub_state_results, manager, state, config.contact_request); if (!sub_state_results.empty()) { found = true; - processInterpolatedSubSegmentCollisionResults( - state_results, sub_state_results, 0, 0, manager.getActiveCollisionObjects(), true); + state_results.addInterpolatedCollisionResults( + sub_state_results, 0, 0, manager.getActiveCollisionObjects(), 0, true); if (console_bridge::getLogLevel() > console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO) { @@ -795,9 +758,10 @@ bool checkTrajectory(std::vector& contact ss << std::endl << " State0: " << traj.row(iStep) << std::endl; - CONSOLE_BRIDGE_logError(ss.str().c_str()); + CONSOLE_BRIDGE_logDebug(ss.str().c_str()); } } + contacts.push_back(state_results); if (found && (config.contact_request.type == tesseract_collision::ContactTestType::FIRST)) break; diff --git a/tesseract_environment/test/tesseract_environment_unit.cpp b/tesseract_environment/test/tesseract_environment_unit.cpp index 131ca660f0a..08d7424b02a 100644 --- a/tesseract_environment/test/tesseract_environment_unit.cpp +++ b/tesseract_environment/test/tesseract_environment_unit.cpp @@ -2715,22 +2715,18 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT std::vector contacts; EXPECT_TRUE( tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); - EXPECT_EQ(contacts[4].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE( tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); } @@ -2784,22 +2780,18 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT std::vector contacts; EXPECT_TRUE( tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); - EXPECT_EQ(contacts[4].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE( tesseract_environment::checkTrajectory(contacts, *discrete_manager, *state_solver, joint_names, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); } @@ -2867,21 +2859,17 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT config.contact_request.type = tesseract_collision::ContactTestType::FIRST; std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); - EXPECT_EQ(contacts[4].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); } @@ -2932,21 +2920,17 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT config.longest_valid_segment_length = std::numeric_limits::max(); std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); - EXPECT_EQ(contacts[4].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *discrete_manager, *joint_group, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows())); + EXPECT_EQ(contacts.size(), 2); EXPECT_EQ(contacts[0].size(), 0); EXPECT_EQ(contacts[1].size(), 1); - EXPECT_EQ(contacts[2].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResults(contacts); } @@ -3015,11 +2999,8 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory( contacts, *continuous_manager, *state_solver, joint_names, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); @@ -3027,9 +3008,8 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory( contacts, *continuous_manager, *state_solver, joint_names, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); @@ -3073,11 +3053,8 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory( contacts, *continuous_manager, *state_solver, joint_names, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); @@ -3085,9 +3062,8 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory( contacts, *continuous_manager, *state_solver, joint_names, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); @@ -3153,20 +3129,16 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT config.contact_request.type = tesseract_collision::ContactTestType::FIRST; std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); @@ -3206,20 +3178,16 @@ TEST(TesseractEnvironmentUnit, checkTrajectoryUnit) // NOLINT config.longest_valid_segment_length = std::numeric_limits::max(); std::vector contacts; EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj, config)); - EXPECT_EQ(contacts.size(), static_cast(traj.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); - EXPECT_EQ(contacts[2].size(), 0); - EXPECT_EQ(contacts[3].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); contacts.clear(); EXPECT_TRUE(tesseract_environment::checkTrajectory(contacts, *continuous_manager, *joint_group, traj2, config)); - EXPECT_EQ(contacts.size(), static_cast(traj2.rows() - 1)); + EXPECT_EQ(contacts.size(), 1); EXPECT_EQ(contacts[0].size(), 1); - EXPECT_EQ(contacts[1].size(), 0); EXPECT_EQ(getContactCount(contacts), static_cast(1)); checkProcessInterpolatedResultsNoTime0(contacts[0]); checkProcessInterpolatedResults(contacts); diff --git a/tesseract_environment/test/tesseract_environment_utils.cpp b/tesseract_environment/test/tesseract_environment_utils.cpp index a4627c6d7cf..1e1e247c796 100644 --- a/tesseract_environment/test/tesseract_environment_utils.cpp +++ b/tesseract_environment/test/tesseract_environment_utils.cpp @@ -129,6 +129,7 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN default_config.contact_manager_config.margin_data_override_type = tesseract_common::CollisionMarginOverrideType::REPLACE; + tesseract_collision::ContactResultMap contacts; // Check Discrete { auto config = default_config; @@ -144,7 +145,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN // In collision by default { - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_FALSE(contacts.empty()); } @@ -152,7 +154,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["boxbot_link"] = false; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_TRUE(contacts.empty()); } @@ -160,7 +163,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["boxbot_link"] = true; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_FALSE(contacts.empty()); } @@ -168,7 +172,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["nonexistant_link"] = false; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_FALSE(contacts.empty()); } } @@ -192,8 +197,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN tmap2["test_box_link"].translate(Eigen::Vector3d(0.9, -2, 0)); { - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); // In collision by default EXPECT_FALSE(contacts.empty()); } @@ -202,8 +207,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["boxbot_link"] = false; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); EXPECT_TRUE(contacts.empty()); } @@ -211,8 +216,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["boxbot_link"] = true; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); EXPECT_FALSE(contacts.empty()); } @@ -220,8 +225,8 @@ TEST(TesseractEnvironmentUtils, applyContactManagerConfigObjectEnable) // NOLIN { config.contact_manager_config.modify_object_enabled["nonexistant_link"] = false; manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); EXPECT_FALSE(contacts.empty()); } } @@ -249,6 +254,7 @@ TEST(TesseractEnvironmentUtils, checkTrajectoryState) // NOLINT default_config.contact_manager_config.margin_data_override_type = tesseract_common::CollisionMarginOverrideType::REPLACE; + tesseract_collision::ContactResultMap contacts; // Check Discrete { auto config = default_config; @@ -265,26 +271,30 @@ TEST(TesseractEnvironmentUtils, checkTrajectoryState) // NOLINT // Not in collision { manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config.contact_request); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config.contact_request); EXPECT_TRUE(contacts.empty()); } // In collision if manager->applyContactManagerConfig works correctly { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.1); manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config.contact_request); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config.contact_request); EXPECT_FALSE(contacts.empty()); } // Not collision if checkTrajectoryState applies the config { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.0); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_TRUE(contacts.empty()); } // In collision if checkTrajectoryState applies the config { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.1); - tesseract_collision::ContactResultMap contacts = checkTrajectoryState(*manager, tmap, config); + contacts.clear(); + checkTrajectoryState(contacts, *manager, tmap, config); EXPECT_FALSE(contacts.empty()); } } @@ -310,28 +320,30 @@ TEST(TesseractEnvironmentUtils, checkTrajectoryState) // NOLINT // Not in collision { manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); EXPECT_TRUE(contacts.empty()); } // In collision if manager->applyContactManagerConfig works correctly { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.1); manager->applyContactManagerConfig(config.contact_manager_config); - tesseract_collision::ContactResultMap contacts = - checkTrajectorySegment(*manager, tmap1, tmap2, config.contact_request); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config.contact_request); EXPECT_FALSE(contacts.empty()); } // Not collision if checkTrajectoryState applies the config { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.0); - tesseract_collision::ContactResultMap contacts = checkTrajectorySegment(*manager, tmap1, tmap2, config); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config); EXPECT_TRUE(contacts.empty()); } // In collision if checkTrajectoryState applies the config { config.contact_manager_config.margin_data.setDefaultCollisionMargin(0.1); - tesseract_collision::ContactResultMap contacts = checkTrajectorySegment(*manager, tmap1, tmap2, config); + contacts.clear(); + checkTrajectorySegment(contacts, *manager, tmap1, tmap2, config); EXPECT_FALSE(contacts.empty()); } } From 0de2d51f66aa19282d7119721b17b8472970c5dc Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 29 Mar 2023 10:12:58 -0500 Subject: [PATCH 2/4] Remove reserve(100) in ContactResultMap does not improve performance --- tesseract_collision/core/src/types.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index a1731b22cde..9c30de3826d 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -61,9 +61,6 @@ ContactResult& ContactResultMap::addContactResult(const KeyType& key, ContactRes { ++cnt_; auto& cv = data_[key]; - if (cv.capacity() == 0) - cv.reserve(100); - return cv.emplace_back(std::move(result)); } @@ -72,7 +69,8 @@ ContactResult& ContactResultMap::addContactResult(const KeyType& key, const Mapp assert(!results.empty()); cnt_ += static_cast(results.size()); auto& cv = data_[key]; - cv.reserve(std::max(static_cast(100), cv.size() + results.size())); + ; + cv.reserve(cv.size() + results.size()); cv.insert(cv.end(), results.begin(), results.end()); return cv.back(); } @@ -83,8 +81,6 @@ ContactResult& ContactResultMap::setContactResult(const KeyType& key, ContactRes cnt_ += (1 - static_cast(cv.size())); assert(cnt_ >= 0); cv.clear(); - if (cv.capacity() == 0) - cv.reserve(100); return cv.emplace_back(std::move(result)); } @@ -96,7 +92,7 @@ ContactResult& ContactResultMap::setContactResult(const KeyType& key, const Mapp cnt_ += (static_cast(results.size()) - static_cast(cv.size())); assert(cnt_ >= 0); cv.clear(); - cv.reserve(std::max(static_cast(100), cv.size() + results.size())); + cv.reserve(cv.size() + results.size()); cv.insert(cv.end(), results.begin(), results.end()); return cv.back(); } @@ -109,7 +105,6 @@ void ContactResultMap::addInterpolatedCollisionResults(ContactResultMap& sub_seg bool discrete, const tesseract_collision::ContactResultMap::FilterFn& filter) { - // double segment_dt = (sub_segment_last_index > 0) ? 1.0 / static_cast(sub_segment_last_index) : 0.0; for (auto& pair : sub_segment_results.data_) { // Update cc_time and cc_type From 9375b0a099e0dddac29cbbfc5de280fecd277a43 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Wed, 29 Mar 2023 11:05:47 -0500 Subject: [PATCH 3/4] Avoid multiple memory allocations in PairHash::operator() --- tesseract_common/src/types.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tesseract_common/src/types.cpp b/tesseract_common/src/types.cpp index 6fa4026ba6a..a23929950c3 100644 --- a/tesseract_common/src/types.cpp +++ b/tesseract_common/src/types.cpp @@ -47,7 +47,9 @@ namespace tesseract_common std::size_t PairHash::operator()(const LinkNamesPair& pair) const { thread_local std::string key; - key = pair.first + pair.second; + key.clear(); + key.append(pair.first); + key.append(pair.second); return std::hash()(key); } From 95907f5e3dc0446955a029281786573163921530 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 30 Mar 2023 16:23:19 -0500 Subject: [PATCH 4/4] Add documentation to ContactResultMap --- .../include/tesseract_collision/core/types.h | 28 +++++++++--- tesseract_collision/core/src/types.cpp | 43 +++++++++---------- .../include/tesseract_common/types.h | 12 ++++++ 3 files changed, 56 insertions(+), 27 deletions(-) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index e2e6b09506d..be4a0cb076b 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -132,6 +132,24 @@ struct ContactResult }; using ContactResultVector = tesseract_common::AlignedVector; + +/** + * @brief This structure hold contact results for link pairs + * @details A custom class was implemented to avoid a large number of heap allocations during motion which avoids full + * clearing the map. This class provides methods const container methods for access the internal unordered_map and has + * two distinct different when it comes to the clear, size and release methods. + * + * The clear method does not call clear on the unordered_map but instead it loops over all entries and calls clear on + * the vector being stored. This allows the memory to remain with the map and not get release for each of the vectors + * stored in the map. + * + * The size method loops over the map and counts those that have vectors which are not empty. + * + * The release method actually calls clear on the internal unordered_map relasing all memory. + * + * @todo This should be updated to leverage a object pool for `ContactResultVector` where in the set and add methods + * it would check it the pair exists and if not it would pull from the object pool. + */ class ContactResultMap { public: @@ -160,15 +178,15 @@ class ContactResultMap /** * @brief Set contact results for the provided key - * @param key The key to append the results to - * @param result The results to add + * @param key The key to assign the provided results to + * @param result The results to assign */ ContactResult& setContactResult(const KeyType& key, ContactResult result); /** * @brief Set contact results for the provided key - * @param key The key to append the results to - * @param result The results to add + * @param key The key to assign the provided results to + * @param result The results to assign */ ContactResult& setContactResult(const KeyType& key, const MappedType& results); @@ -261,7 +279,7 @@ class ContactResultMap private: ContainerType data_; - long cnt_{ 0 }; + long count_{ 0 }; }; /** diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 9c30de3826d..9d76bbfb054 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -59,7 +59,7 @@ ContactRequest::ContactRequest(ContactTestType type) : type(type) {} ContactResult& ContactResultMap::addContactResult(const KeyType& key, ContactResult result) { - ++cnt_; + ++count_; auto& cv = data_[key]; return cv.emplace_back(std::move(result)); } @@ -67,9 +67,8 @@ ContactResult& ContactResultMap::addContactResult(const KeyType& key, ContactRes ContactResult& ContactResultMap::addContactResult(const KeyType& key, const MappedType& results) { assert(!results.empty()); - cnt_ += static_cast(results.size()); + count_ += static_cast(results.size()); auto& cv = data_[key]; - ; cv.reserve(cv.size() + results.size()); cv.insert(cv.end(), results.begin(), results.end()); return cv.back(); @@ -78,8 +77,8 @@ ContactResult& ContactResultMap::addContactResult(const KeyType& key, const Mapp ContactResult& ContactResultMap::setContactResult(const KeyType& key, ContactResult result) { auto& cv = data_[key]; - cnt_ += (1 - static_cast(cv.size())); - assert(cnt_ >= 0); + count_ += (1 - static_cast(cv.size())); + assert(count_ >= 0); cv.clear(); return cv.emplace_back(std::move(result)); @@ -89,10 +88,10 @@ ContactResult& ContactResultMap::setContactResult(const KeyType& key, const Mapp { assert(!results.empty()); auto& cv = data_[key]; - cnt_ += (static_cast(results.size()) - static_cast(cv.size())); - assert(cnt_ >= 0); + count_ += (static_cast(results.size()) - static_cast(cv.size())); + assert(count_ >= 0); cv.clear(); - cv.reserve(cv.size() + results.size()); + cv.reserve(results.size()); cv.insert(cv.end(), results.begin(), results.end()); return cv.back(); } @@ -142,7 +141,7 @@ void ContactResultMap::addInterpolatedCollisionResults(ContactResultMap& sub_seg if (!pair.second.empty()) { // Add results to the full segment results - cnt_ += static_cast(pair.second.size()); + count_ += static_cast(pair.second.size()); auto it = data_.find(pair.first); if (it == data_.end()) { @@ -162,11 +161,11 @@ void ContactResultMap::addInterpolatedCollisionResults(ContactResultMap& sub_seg } } -long ContactResultMap::count() const { return cnt_; } +long ContactResultMap::count() const { return count_; } std::size_t ContactResultMap::size() const { - if (cnt_ == 0) + if (count_ == 0) return 0; std::size_t cnt{ 0 }; @@ -179,24 +178,24 @@ std::size_t ContactResultMap::size() const return cnt; } -bool ContactResultMap::empty() const { return (cnt_ == 0); } +bool ContactResultMap::empty() const { return (count_ == 0); } void ContactResultMap::clear() { - if (cnt_ == 0) + if (count_ == 0) return; // Only clear the vectors so the capacity stays the same for (auto& cv : data_) cv.second.clear(); - cnt_ = 0; + count_ = 0; } void ContactResultMap::release() { data_.clear(); - cnt_ = 0; + count_ = 0; } const ContactResultMap::ContainerType& ContactResultMap::getContainer() const { return data_; } @@ -216,19 +215,19 @@ ContactResultMap::ConstIteratorType ContactResultMap::find(const KeyType& key) c void ContactResultMap::flattenMoveResults(ContactResultVector& v) { v.clear(); - v.reserve(static_cast(cnt_)); + v.reserve(static_cast(count_)); for (auto& mv : data_) { std::move(mv.second.begin(), mv.second.end(), std::back_inserter(v)); mv.second.clear(); } - cnt_ = 0; + count_ = 0; } void ContactResultMap::flattenCopyResults(ContactResultVector& v) const { v.clear(); - v.reserve(static_cast(cnt_)); + v.reserve(static_cast(count_)); for (const auto& mv : data_) std::copy(mv.second.begin(), mv.second.end(), std::back_inserter(v)); } @@ -236,7 +235,7 @@ void ContactResultMap::flattenCopyResults(ContactResultVector& v) const void ContactResultMap::flattenWrapperResults(std::vector>& v) { v.clear(); - v.reserve(static_cast(cnt_)); + v.reserve(static_cast(count_)); for (auto& mv : data_) v.insert(v.end(), mv.second.begin(), mv.second.end()); } @@ -244,7 +243,7 @@ void ContactResultMap::flattenWrapperResults(std::vector>& v) const { v.clear(); - v.reserve(static_cast(cnt_)); + v.reserve(static_cast(count_)); for (const auto& mv : data_) v.insert(v.end(), mv.second.begin(), mv.second.end()); } @@ -258,8 +257,8 @@ void ContactResultMap::filter(const FilterFn& filter) filter(pair); removed_cnt += (current_cnt - pair.second.size()); } - cnt_ -= static_cast(removed_cnt); - assert(cnt_ >= 0); + count_ -= static_cast(removed_cnt); + assert(count_ >= 0); } ContactTestData::ContactTestData(const std::vector& active, diff --git a/tesseract_common/include/tesseract_common/types.h b/tesseract_common/include/tesseract_common/types.h index bb4827ce5e7..60cff1e79e7 100644 --- a/tesseract_common/include/tesseract_common/types.h +++ b/tesseract_common/include/tesseract_common/types.h @@ -84,6 +84,18 @@ struct PairHash * @return LinkNamesPair a lexicographically sorted pair of strings */ LinkNamesPair makeOrderedLinkPair(const std::string& link_name1, const std::string& link_name2); + +/** + * @brief Populate a pair of strings, where the pair.first is always <= pair.second. + * + * This is used to avoid multiple memory application throughout the code base + * + * This is commonly used along with PairHash as the key to an unordered_map + * + * @param pair The link name pair to load a lexicographically sorted pair of strings + * @param link_name1 First link name + * @param link_name2 Second link nam + */ void makeOrderedLinkPair(LinkNamesPair& pair, const std::string& link_name1, const std::string& link_name2); /** @brief The Plugin Information struct */