Add yaml conversions and related tests

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)

trajopt_common/include/trajopt_common/collision_types.h

@@ -47,7 +47,20 @@ struct CollisionCoeffData
   using Ptr = std::shared_ptr<CollisionCoeffData>;
   using ConstPtr = std::shared_ptr<const CollisionCoeffData>;
 
-  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<tesseract_common::LinkNamesPair, double>& 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<tesseract_common::LinkNamesPair, double> lookup_table_;

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/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <yaml-cpp/yaml.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <trajopt_common/collision_types.h>
+#include <tesseract_collision/core/yaml_extensions.h>
+#include <tesseract_common/yaml_extensions.h>
+
+namespace YAML
+{
+//=========================== CollisionCoeffData ===========================
+template <>
+struct convert<trajopt_common::CollisionCoeffData>
+{
+  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<double>());
+
+    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<std::string>(), key_node[1].as<std::string>(), it->second.as<double>());
+      }
+    }
+
+    return true;
+  }
+};
+
+//=========================== TrajOptCollisionConfig ===========================
+template <>
+struct convert<trajopt_common::TrajOptCollisionConfig>
+{
+  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<bool>();
+    if (const YAML::Node& n = node["contact_manager_config"])
+      rhs.contact_manager_config = n.as<tesseract_collision::ContactManagerConfig>();
+    if (const YAML::Node& n = node["collision_check_config"])
+      rhs.collision_check_config = n.as<tesseract_collision::CollisionCheckConfig>();
+    if (const YAML::Node& n = node["collision_coeff_data"])
+      rhs.collision_coeff_data = n.as<trajopt_common::CollisionCoeffData>();
+
+    // Accept both 'collision_margin_buffer' and legacy alias 'buffer'
+    if (const YAML::Node& n = node["collision_margin_buffer"])
+      rhs.collision_margin_buffer = n.as<double>();
+
+    if (const YAML::Node& n = node["max_num_cnt"])
+      rhs.max_num_cnt = n.as<int>();
+
+    // Optional: scale contact manager margins if provided
+    if (const YAML::Node& n = node["scale_margins"])
+      rhs.contact_manager_config.scaleMargins(n.as<double>());
+
+    return true;
+  }
+};
+
+}  // namespace YAML
+
+#endif  // TRAJOPT_COMMON_YAML_EXTENSIONS_H

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<tesseract_common::LinkNamesPair, double>& CollisionCoeffData::getCollisionCoeffPairData() const
+{
+  return lookup_table_;
+}
+
 const std::set<tesseract_common::LinkNamesPair>& CollisionCoeffData::getPairsWithZeroCoeff() const
 {
   return zero_coeff_;

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)