diff --git a/trajopt_common/CMakeLists.txt b/trajopt_common/CMakeLists.txt index 09beab64..e67a3e2f 100644 --- a/trajopt_common/CMakeLists.txt +++ b/trajopt_common/CMakeLists.txt @@ -58,6 +58,12 @@ install( PATTERN "*.hpp" PATTERN ".svn" EXCLUDE) +if(TRAJOPT_ENABLE_TESTING) + enable_testing() + add_custom_target(run_tests) + add_subdirectory(test) +endif() + install(FILES "${CMAKE_CURRENT_LIST_DIR}/cmake/trajopt_macros.cmake" DESTINATION lib/cmake/${PROJECT_NAME}) foreach(dir data) diff --git a/trajopt_common/include/trajopt_common/collision_types.h b/trajopt_common/include/trajopt_common/collision_types.h index cdecba51..1c7cbff9 100644 --- a/trajopt_common/include/trajopt_common/collision_types.h +++ b/trajopt_common/include/trajopt_common/collision_types.h @@ -47,7 +47,20 @@ struct CollisionCoeffData using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - CollisionCoeffData(double default_collision_coeff = 1); + CollisionCoeffData() = default; + CollisionCoeffData(double default_collision_coeff); + + /** + * @brief Set the default collision coefficient + * @param default_collision_coeff The default collision coefficient used when no pair-specific coefficient is set + */ + void setDefaultCollisionCoeff(double default_collision_coeff); + + /** + * @brief Get the default collision coefficient + * @return The default collision coefficient used when no pair-specific coefficient is set + */ + double getDefaultCollisionCoeff() const; /** * @brief Set the coefficient for a given contact pair @@ -72,6 +85,12 @@ struct CollisionCoeffData */ double getCollisionCoeff(const std::string& obj1, const std::string& obj2) const; + /** + * @brief Get all collision coefficient pair data + * @return A reference to the lookup table containing all pair-specific coefficients + */ + const std::unordered_map& getCollisionCoeffPairData() const; + /** * @brief Get the pairs with zero coeff * @return A vector of pairs with zero coeff @@ -80,7 +99,7 @@ struct CollisionCoeffData private: /// Stores the collision coefficient used if no pair-specific one is set - double default_collision_coeff_; + double default_collision_coeff_{ 1 }; /// A map of link pair names to contact distance std::unordered_map lookup_table_; diff --git a/trajopt_common/include/trajopt_common/yaml_extensions.h b/trajopt_common/include/trajopt_common/yaml_extensions.h new file mode 100644 index 00000000..df3f5737 --- /dev/null +++ b/trajopt_common/include/trajopt_common/yaml_extensions.h @@ -0,0 +1,145 @@ +/** + * @file yaml_extensions.h + * @brief YAML Type conversions + * + * @author Tyler Marr + * @date August 8, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Tyler Marr, Confinity Robotics + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef TRAJOPT_COMMON_YAML_EXTENSIONS_H +#define TRAJOPT_COMMON_YAML_EXTENSIONS_H + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +namespace YAML +{ +//=========================== CollisionCoeffData =========================== +template <> +struct convert +{ + static Node encode(const trajopt_common::CollisionCoeffData& rhs) + { + Node node; + + // Encode default coefficient + node["default_coeff"] = rhs.getDefaultCollisionCoeff(); + + // Encode all pair-specific coefficients + const auto& pair_data = rhs.getCollisionCoeffPairData(); + if (!pair_data.empty()) + { + Node pair_coeff_data_node(YAML::NodeType::Sequence); + for (const auto& pair : pair_data) + { + Node key_node(NodeType::Sequence); + key_node.push_back(pair.first.first); + key_node.push_back(pair.first.second); + + // tell yaml-cpp “emit this sequence in [a, b] inline style” + key_node.SetStyle(YAML::EmitterStyle::Flow); + + pair_coeff_data_node[key_node] = pair.second; + } + node["pair_coeff_data"] = pair_coeff_data_node; + } + + return node; + } + + static bool decode(const Node& node, trajopt_common::CollisionCoeffData& rhs) + { + if (!node.IsMap()) + return false; + + if (const YAML::Node& n = node["default_coeff"]) + rhs.setDefaultCollisionCoeff(n.as()); + + if (const YAML::Node& pair_coeff_data_node = node["pair_coeff_data"]) + { + for (auto it = pair_coeff_data_node.begin(); it != pair_coeff_data_node.end(); ++it) + { + Node key_node = it->first; + if (!key_node.IsSequence() || key_node.size() != 2) + return false; + + rhs.setCollisionCoeff(key_node[0].as(), key_node[1].as(), it->second.as()); + } + } + + return true; + } +}; + +//=========================== TrajOptCollisionConfig =========================== +template <> +struct convert +{ + static Node encode(const trajopt_common::TrajOptCollisionConfig& rhs) + { + Node node; + node["enabled"] = rhs.enabled; + node["contact_manager_config"] = rhs.contact_manager_config; + node["collision_check_config"] = rhs.collision_check_config; + node["collision_coeff_data"] = rhs.collision_coeff_data; + node["collision_margin_buffer"] = rhs.collision_margin_buffer; + node["max_num_cnt"] = rhs.max_num_cnt; + return node; + } + + static bool decode(const Node& node, trajopt_common::TrajOptCollisionConfig& rhs) + { + if (!node.IsMap()) + return false; + + if (const YAML::Node& n = node["enabled"]) + rhs.enabled = n.as(); + if (const YAML::Node& n = node["contact_manager_config"]) + rhs.contact_manager_config = n.as(); + if (const YAML::Node& n = node["collision_check_config"]) + rhs.collision_check_config = n.as(); + if (const YAML::Node& n = node["collision_coeff_data"]) + rhs.collision_coeff_data = n.as(); + + // Accept both 'collision_margin_buffer' and legacy alias 'buffer' + if (const YAML::Node& n = node["collision_margin_buffer"]) + rhs.collision_margin_buffer = n.as(); + + if (const YAML::Node& n = node["max_num_cnt"]) + rhs.max_num_cnt = n.as(); + + // Optional: scale contact manager margins if provided + if (const YAML::Node& n = node["scale_margins"]) + rhs.contact_manager_config.scaleMargins(n.as()); + + return true; + } +}; + +} // namespace YAML + +#endif // TRAJOPT_COMMON_YAML_EXTENSIONS_H diff --git a/trajopt_common/src/collision_types.cpp b/trajopt_common/src/collision_types.cpp index 1f84c385..7aa2218f 100644 --- a/trajopt_common/src/collision_types.cpp +++ b/trajopt_common/src/collision_types.cpp @@ -37,6 +37,13 @@ CollisionCoeffData::CollisionCoeffData(double default_collision_coeff) { } +void CollisionCoeffData::setDefaultCollisionCoeff(double default_collision_coeff) +{ + default_collision_coeff_ = default_collision_coeff; +} + +double CollisionCoeffData::getDefaultCollisionCoeff() const { return default_collision_coeff_; } + void CollisionCoeffData::setCollisionCoeff(const std::string& obj1, const std::string& obj2, double collision_coeff) { auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2); @@ -59,6 +66,11 @@ double CollisionCoeffData::getCollisionCoeff(const std::string& obj1, const std: return default_collision_coeff_; } +const std::unordered_map& CollisionCoeffData::getCollisionCoeffPairData() const +{ + return lookup_table_; +} + const std::set& CollisionCoeffData::getPairsWithZeroCoeff() const { return zero_coeff_; diff --git a/trajopt_common/test/CMakeLists.txt b/trajopt_common/test/CMakeLists.txt new file mode 100644 index 00000000..00eace8e --- /dev/null +++ b/trajopt_common/test/CMakeLists.txt @@ -0,0 +1,19 @@ +find_package(GTest REQUIRED) + +# TrajOpt Common YAML Unit tests +add_executable(${PROJECT_NAME}_yaml_unit trajopt_common_yaml_conversions_tests.cpp) +target_link_libraries(${PROJECT_NAME}_yaml_unit PRIVATE GTest::GTest GTest::Main ${PROJECT_NAME}) +target_compile_options(${PROJECT_NAME}_yaml_unit PRIVATE ${TRAJOPT_COMPILE_OPTIONS_PRIVATE} + ${TRAJOPT_COMPILE_OPTIONS_PUBLIC}) +target_compile_definitions(${PROJECT_NAME}_yaml_unit PRIVATE ${TRAJOPT_COMPILE_DEFINITIONS}) +target_clang_tidy(${PROJECT_NAME}_yaml_unit ENABLE ${TRAJOPT_ENABLE_CLANG_TIDY}) +target_cxx_version(${PROJECT_NAME}_yaml_unit PRIVATE VERSION ${TRAJOPT_CXX_VERSION}) +target_code_coverage( + ${PROJECT_NAME}_yaml_unit + PRIVATE + ALL + EXCLUDE ${COVERAGE_EXCLUDE} + ENABLE ${TRAJOPT_ENABLE_CODE_COVERAGE}) +add_gtest_discover_tests(${PROJECT_NAME}_yaml_unit) +add_dependencies(${PROJECT_NAME}_yaml_unit ${PROJECT_NAME}) +add_dependencies(run_tests ${PROJECT_NAME}_yaml_unit) diff --git a/trajopt_common/test/trajopt_common_yaml_conversions_tests.cpp b/trajopt_common/test/trajopt_common_yaml_conversions_tests.cpp new file mode 100644 index 00000000..3e5c4479 --- /dev/null +++ b/trajopt_common/test/trajopt_common_yaml_conversions_tests.cpp @@ -0,0 +1,234 @@ +/** + * @file trajopt_common_yaml_conversions_tests.cpp + * @brief This contains unit test for TrajOpt YAML conversions + * + * @author Tyler Marr + * @date August 8, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Tyler Marr, Confinity Robotics + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include +#include + +#include + +class TrajoptCommonYAMLTestFixture : public ::testing::Test +{ +public: + TrajoptCommonYAMLTestFixture() = default; + using ::testing::Test::Test; +}; + +TEST(TrajoptCommonYAMLTestFixture, CollisionCoeffDataConversionsUnit) // NOLINT +{ + const std::string yaml_string = R"( + default_coeff: 2.5 + pair_coeff_data: + [link_a, link_b]: 0.0 + [link_c, link_d]: 1.5 + )"; + + { // decode + YAML::Node n = YAML::Load(yaml_string); + auto d = n.as(); + + // default for unknown + EXPECT_NEAR(d.getCollisionCoeff("foo", "bar"), 2.5, 1e-8); + // specified + EXPECT_NEAR(d.getCollisionCoeff("link_c", "link_d"), 1.5, 1e-8); + EXPECT_NEAR(d.getCollisionCoeff("link_a", "link_b"), 0.0, 1e-8); + } + + { // encode + trajopt_common::CollisionCoeffData data_original(3.0); + data_original.setCollisionCoeff("link_a", "link_b", 0.0); + data_original.setCollisionCoeff("link_c", "link_d", 1.5); + + YAML::Node n(data_original); + auto data = n.as(); + EXPECT_NEAR(data.getDefaultCollisionCoeff(), 3.0, 1e-8); + EXPECT_NEAR(data.getCollisionCoeff("link_a", "link_b"), 0.0, 1e-8); + EXPECT_NEAR(data.getCollisionCoeff("link_c", "link_d"), 1.5, 1e-8); + EXPECT_EQ(data.getPairsWithZeroCoeff().size(), 1U); + EXPECT_EQ(data.getCollisionCoeffPairData().size(), 2U); + } +} + +TEST(TrajoptCommonYAMLTestFixture, TrajOptCollisionConfigConversionsUnit) // NOLINT +{ + const std::string yaml_string = R"( + enabled: false + contact_manager_config: + default_margin: 0.02 + collision_check_config: + type: DISCRETE + longest_valid_segment_length: 0.01 + check_program_mode: ALL + collision_coeff_data: + default_coeff: 3.0 + pair_coeff_data: + [l0, l1]: 0.0 + collision_margin_buffer: 0.05 + max_num_cnt: 10 + scale_margins: 0.5 + )"; + + { // decode + YAML::Node n = YAML::Load(yaml_string); + auto c = n.as(); + + EXPECT_FALSE(c.enabled); + // default_margin scaled by 0.5 + EXPECT_TRUE(c.contact_manager_config.default_margin.has_value()); + // NOLINTNEXTLINE + EXPECT_NEAR(c.contact_manager_config.default_margin.value(), 0.01, 1e-8); + EXPECT_NEAR(c.collision_check_config.longest_valid_segment_length, 0.01, 1e-8); + EXPECT_NEAR(c.collision_margin_buffer, 0.05, 1e-8); + EXPECT_EQ(c.max_num_cnt, 10); + + // coeff data + EXPECT_NEAR(c.collision_coeff_data.getCollisionCoeff("foo", "bar"), 3.0, 1e-8); + EXPECT_NEAR(c.collision_coeff_data.getCollisionCoeff("l0", "l1"), 0.0, 1e-8); + } + + { // encode + trajopt_common::TrajOptCollisionConfig data_original; + data_original.enabled = false; + data_original.contact_manager_config.default_margin = 0.01; + data_original.collision_check_config.longest_valid_segment_length = 0.02; + data_original.collision_coeff_data = trajopt_common::CollisionCoeffData(2.0); + data_original.collision_coeff_data.setCollisionCoeff("l0", "l1", 0.0); + data_original.collision_coeff_data.setCollisionCoeff("l1", "l2", 1.7); + data_original.collision_margin_buffer = 0.1; + data_original.max_num_cnt = 5; + + YAML::Node n(data_original); + auto data = n.as(); + + EXPECT_FALSE(data.enabled); + EXPECT_TRUE(data.contact_manager_config.default_margin.has_value()); + // NOLINTNEXTLINE + EXPECT_NEAR(data.contact_manager_config.default_margin.value(), 0.01, 1e-8); + EXPECT_NEAR(data.collision_check_config.longest_valid_segment_length, 0.02, 1e-8); + EXPECT_NEAR(data.collision_margin_buffer, 0.1, 1e-8); + EXPECT_EQ(data.max_num_cnt, 5); + + EXPECT_NEAR(data.collision_coeff_data.getDefaultCollisionCoeff(), 2.0, 1e-8); + EXPECT_NEAR(data.collision_coeff_data.getCollisionCoeff("l0", "l1"), 0.0, 1e-8); + EXPECT_NEAR(data.collision_coeff_data.getCollisionCoeff("l1", "l2"), 1.7, 1e-8); + EXPECT_EQ(data.collision_coeff_data.getPairsWithZeroCoeff().size(), 1U); + EXPECT_EQ(data.collision_coeff_data.getCollisionCoeffPairData().size(), 2U); + } +} + +TEST(TrajoptCommonYAMLTestFixture, CollisionCoeffDataGetterMethodsUnit) // NOLINT +{ + trajopt_common::CollisionCoeffData d(3.5); + d.setCollisionCoeff("link1", "link2", 2.0); + d.setCollisionCoeff("link3", "link4", 0.0); + d.setCollisionCoeff("link5", "link6", 1.5); + + // Test getDefaultCollisionCoeff + EXPECT_NEAR(d.getDefaultCollisionCoeff(), 3.5, 1e-12); + + // Test getCollisionCoeffPairData + const auto& pair_data = d.getCollisionCoeffPairData(); + EXPECT_EQ(pair_data.size(), 3U); + + // Verify the pairs are stored correctly (note: pairs are ordered internally) + EXPECT_NEAR(d.getCollisionCoeff("link1", "link2"), 2.0, 1e-12); + EXPECT_NEAR(d.getCollisionCoeff("link3", "link4"), 0.0, 1e-12); + EXPECT_NEAR(d.getCollisionCoeff("link5", "link6"), 1.5, 1e-12); + + // Test getPairsWithZeroCoeff + const auto& zero_pairs = d.getPairsWithZeroCoeff(); + EXPECT_EQ(zero_pairs.size(), 1U); + + // Verify that the zero coefficient pair is in the zero_pairs set + bool found_zero_pair = false; + for (const auto& pair : zero_pairs) + { + if ((pair.first == "link3" && pair.second == "link4") || (pair.first == "link4" && pair.second == "link3")) + { + found_zero_pair = true; + break; + } + } + EXPECT_TRUE(found_zero_pair); +} + +TEST(TrajoptCommonYAMLTestFixture, CollisionCoeffDataRoundTripUnit) // NOLINT +{ + trajopt_common::CollisionCoeffData d_in(2.5); + d_in.setCollisionCoeff("a", "b", 0.0); + d_in.setCollisionCoeff("c", "d", 1.8); + + YAML::Node n(d_in); + auto d_out = n.as(); + EXPECT_NEAR(d_out.getCollisionCoeff("x", "y"), 2.5, 1e-12); + EXPECT_NEAR(d_out.getCollisionCoeff("a", "b"), 0.0, 1e-12); + EXPECT_NEAR(d_out.getCollisionCoeff("b", "a"), 0.0, 1e-12); + EXPECT_NEAR(d_out.getCollisionCoeff("c", "d"), 1.8, 1e-12); + EXPECT_NEAR(d_out.getCollisionCoeff("d", "c"), 1.8, 1e-12); +} + +TEST(TrajoptCommonYAMLTestFixture, TrajOptCollisionConfigRoundTripUnit) // NOLINT +{ + trajopt_common::TrajOptCollisionConfig c_in; + c_in.enabled = false; + c_in.contact_manager_config.default_margin = 0.02; + c_in.collision_check_config.longest_valid_segment_length = 0.01; + c_in.collision_coeff_data = trajopt_common::CollisionCoeffData(3.3); + c_in.collision_coeff_data.setCollisionCoeff("l0", "l1", 0.0); + c_in.collision_margin_buffer = 0.05; + c_in.max_num_cnt = 7; + + YAML::Node n(c_in); + auto c_out = n.as(); + + EXPECT_EQ(c_out.enabled, c_in.enabled); + ASSERT_TRUE(c_out.contact_manager_config.default_margin.has_value()); + ASSERT_TRUE(c_in.contact_manager_config.default_margin.has_value()); + + // NOLINTNEXTLINE + EXPECT_NEAR( + c_out.contact_manager_config.default_margin.value(), c_in.contact_manager_config.default_margin.value(), 1e-12); + + EXPECT_NEAR(c_out.collision_check_config.longest_valid_segment_length, + c_in.collision_check_config.longest_valid_segment_length, + 1e-12); + EXPECT_NEAR(c_out.collision_coeff_data.getCollisionCoeff("x", "y"), 3.3, 1e-12); + EXPECT_NEAR(c_out.collision_coeff_data.getCollisionCoeff("l0", "l1"), 0.0, 1e-12); + EXPECT_NEAR(c_out.collision_margin_buffer, c_in.collision_margin_buffer, 1e-12); + EXPECT_EQ(c_out.max_num_cnt, c_in.max_num_cnt); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}