From 840fec965051645f5a085c47928c2063f047ff4d Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Mon, 3 Jun 2024 14:48:55 -0500 Subject: [PATCH 1/4] Add ability to briefly summarize trajectory collisions --- .../include/tesseract_collision/core/types.h | 2 + tesseract_collision/core/src/types.cpp | 47 +++++++++++++++++++ 2 files changed, 49 insertions(+) diff --git a/tesseract_collision/core/include/tesseract_collision/core/types.h b/tesseract_collision/core/include/tesseract_collision/core/types.h index 44e2115ca6a..b79f13ed01b 100644 --- a/tesseract_collision/core/include/tesseract_collision/core/types.h +++ b/tesseract_collision/core/include/tesseract_collision/core/types.h @@ -560,6 +560,8 @@ struct ContactTrajectoryResults std::stringstream trajectoryCollisionResultsTable() const; + std::stringstream collisionFrequencyPerLink() const; + std::vector steps; std::vector joint_names; int total_steps = 0; diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 88a96db717d..3d0c656eb41 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -708,4 +708,51 @@ std::stringstream ContactTrajectoryResults::trajectoryCollisionResultsTable() co return ss; } +std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const +{ + // Count all links that experienced a collision + std::unordered_map link_collision_count; + for (const auto& step : steps) + { + for (const auto& substep : step.substeps) + { + for (const auto& contact_pair : substep.contacts.getContainer()) + { + const auto& link_pair = contact_pair.first; + link_collision_count[link_pair.first]++; + link_collision_count[link_pair.second]++; + } + } + } + + // Determine the maximum width for the link name column to have a clean output + size_t max_link_name_length = 0; + for (const auto& entry : link_collision_count) + { + if (entry.first.size() > max_link_name_length) + max_link_name_length = entry.first.size(); + } + + // Adjust the width to have some extra space after the longest link name + const size_t column_width = max_link_name_length + 2; + + // Create a vector of pairs and sort it by frequency in descending order + std::vector> sorted_collisions(link_collision_count.begin(), link_collision_count.end()); + std::sort(sorted_collisions.begin(), sorted_collisions.end(), [](const auto& a, const auto& b) { + return b.second < a.second; + }); + + // Create a string stream to store the table + std::stringstream ss; + ss << std::left << std::setw(static_cast(column_width)) << "Link Name" + << "Collisions" << std::endl; + ss << std::string(column_width + 10, '-') << std::endl; + for (const auto& entry : sorted_collisions) + { + ss << std::left << std::setw(static_cast(column_width)) << entry.first << entry.second << std::endl; + } + + return ss; +} + } // namespace tesseract_collision From 9e498b8d64e02e1083cdfd4c55f558ba9575ac36 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 7 Jun 2024 18:37:59 -0500 Subject: [PATCH 2/4] Change to using a table format to summarize link collisions --- tesseract_collision/core/src/types.cpp | 101 ++++++++++++++++++++----- 1 file changed, 82 insertions(+), 19 deletions(-) diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 3d0c656eb41..8a2767911c0 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -710,8 +710,9 @@ std::stringstream ContactTrajectoryResults::trajectoryCollisionResultsTable() co std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const { - // Count all links that experienced a collision - std::unordered_map link_collision_count; + // Create a map to assign an index to each unique link name + std::unordered_map link_index_map; + int index = 0; for (const auto& step : steps) { for (const auto& substep : step.substeps) @@ -719,37 +720,99 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const for (const auto& contact_pair : substep.contacts.getContainer()) { const auto& link_pair = contact_pair.first; - link_collision_count[link_pair.first]++; - link_collision_count[link_pair.second]++; + if (link_index_map.find(link_pair.first) == link_index_map.end()) + { + link_index_map[link_pair.first] = index++; + } + if (link_index_map.find(link_pair.second) == link_index_map.end()) + { + link_index_map[link_pair.second] = index++; + } } } } - // Determine the maximum width for the link name column to have a clean output + // Create a matrix to store the collision counts + std::size_t size = link_index_map.size(); + std::vector> collision_matrix(size, std::vector(size, 0)); + + // Populate the matrix with collision counts + for (const auto& step : steps) + { + for (const auto& substep : step.substeps) + { + for (const auto& contact_pair : substep.contacts.getContainer()) + { + const auto& link_pair = contact_pair.first; + std::size_t row = link_index_map[link_pair.first]; + std::size_t col = link_index_map[link_pair.second]; + collision_matrix[row][col]++; + collision_matrix[col][row]++; + } + } + } + + // Create a string stream to store the matrix + std::stringstream ss; + + if (link_index_map.empty()) + { + ss << "No contacts detected" << std::endl; + return ss; + } + + // Determine the maximum width for the link name column size_t max_link_name_length = 0; - for (const auto& entry : link_collision_count) + for (const auto& entry : link_index_map) { if (entry.first.size() > max_link_name_length) max_link_name_length = entry.first.size(); } // Adjust the width to have some extra space after the longest link name - const size_t column_width = max_link_name_length + 2; + const int column_width = static_cast(max_link_name_length) + 2; + + // Prepare the header row + ss << std::setw(column_width + 5) << " " + << "|"; + for (size_t i = 0; i < link_index_map.size(); ++i) + { + ss << std::setw(5) << i << "|"; + } + ss << std::endl; - // Create a vector of pairs and sort it by frequency in descending order - std::vector> sorted_collisions(link_collision_count.begin(), link_collision_count.end()); - std::sort(sorted_collisions.begin(), sorted_collisions.end(), [](const auto& a, const auto& b) { - return b.second < a.second; - }); + // Prepare the separator row + ss << std::setw(column_width + 5) << " " + << "|"; + for (size_t i = 0; i < link_index_map.size(); ++i) + { + ss << std::setw(5) << "-----" + << "|"; + } + ss << std::endl; - // Create a string stream to store the table - std::stringstream ss; - ss << std::left << std::setw(static_cast(column_width)) << "Link Name" - << "Collisions" << std::endl; - ss << std::string(column_width + 10, '-') << std::endl; - for (const auto& entry : sorted_collisions) + // Prepare the data rows + std::vector link_names(link_index_map.size()); + for (const auto& entry : link_index_map) { - ss << std::left << std::setw(static_cast(column_width)) << entry.first << entry.second << std::endl; + link_names[entry.second] = entry.first; + } + + for (size_t i = 0; i < link_names.size(); ++i) + { + ss << std::setw(5) << i << std::setw(column_width) << link_names[i] << "|"; + for (size_t j = 0; j < link_names.size(); ++j) + { + if (i == j) + { + break; + } + else + { + ss << std::setw(5) << collision_matrix[i][j] << "|"; + } + } + ss << std::endl; } return ss; From 52a15cfc6bb579020bd050667fd9973b1ebd0bdc Mon Sep 17 00:00:00 2001 From: Tyler Marr <41449746+marrts@users.noreply.github.com> Date: Sat, 8 Jun 2024 08:14:14 -0500 Subject: [PATCH 3/4] Change index to std::size_t --- tesseract_collision/core/src/types.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index 8a2767911c0..bf5a2eb654f 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -712,7 +712,7 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const { // Create a map to assign an index to each unique link name std::unordered_map link_index_map; - int index = 0; + std::size_t index = 0; for (const auto& step : steps) { for (const auto& substep : step.substeps) From 5cf57decb8da0076cee083e9acd3d20b786777a9 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 10 Jun 2024 11:59:17 -0500 Subject: [PATCH 4/4] fix clang-tidy errors --- tesseract_collision/core/src/types.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/tesseract_collision/core/src/types.cpp b/tesseract_collision/core/src/types.cpp index bf5a2eb654f..f9631605e7d 100644 --- a/tesseract_collision/core/src/types.cpp +++ b/tesseract_collision/core/src/types.cpp @@ -762,7 +762,7 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const } // Determine the maximum width for the link name column - size_t max_link_name_length = 0; + std::size_t max_link_name_length = 0; for (const auto& entry : link_index_map) { if (entry.first.size() > max_link_name_length) @@ -775,7 +775,7 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const // Prepare the header row ss << std::setw(column_width + 5) << " " << "|"; - for (size_t i = 0; i < link_index_map.size(); ++i) + for (std::size_t i = 0; i < link_index_map.size(); ++i) { ss << std::setw(5) << i << "|"; } @@ -784,7 +784,7 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const // Prepare the separator row ss << std::setw(column_width + 5) << " " << "|"; - for (size_t i = 0; i < link_index_map.size(); ++i) + for (std::size_t i = 0; i < link_index_map.size(); ++i) { ss << std::setw(5) << "-----" << "|"; @@ -798,19 +798,15 @@ std::stringstream ContactTrajectoryResults::collisionFrequencyPerLink() const link_names[entry.second] = entry.first; } - for (size_t i = 0; i < link_names.size(); ++i) + for (std::size_t i = 0; i < link_names.size(); ++i) { ss << std::setw(5) << i << std::setw(column_width) << link_names[i] << "|"; - for (size_t j = 0; j < link_names.size(); ++j) + for (std::size_t j = 0; j < link_names.size(); ++j) { if (i == j) - { break; - } - else - { - ss << std::setw(5) << collision_matrix[i][j] << "|"; - } + + ss << std::setw(5) << collision_matrix[i][j] << "|"; } ss << std::endl; }