From d987e67b50181c2365242460c2b55374dda48157 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 20 May 2026 11:39:57 -0400 Subject: [PATCH 01/10] urdf to model table, model table to tensor, tensor to model table --- CMakeLists.txt | 11 +- conanfile.py | 2 + src/viam/sdk/CMakeLists.txt | 3 + .../private/urdf_model_table_internals.hpp | 43 ++ .../sdk/referenceframe/urdf_model_table.cpp | 251 +++++++++ .../sdk/referenceframe/urdf_model_table.hpp | 56 ++ src/viam/sdk/tests/CMakeLists.txt | 6 + src/viam/sdk/tests/test_urdf_model_table.cpp | 523 ++++++++++++++++++ src/viam/sdk/tests/testfiles/gp12.urdf | 115 ++++ src/viam/sdk/tests/testfiles/ur5e-real.urdf | 270 +++++++++ 10 files changed, 1279 insertions(+), 1 deletion(-) create mode 100644 src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp create mode 100644 src/viam/sdk/referenceframe/urdf_model_table.cpp create mode 100644 src/viam/sdk/referenceframe/urdf_model_table.hpp create mode 100644 src/viam/sdk/tests/test_urdf_model_table.cpp create mode 100644 src/viam/sdk/tests/testfiles/gp12.urdf create mode 100644 src/viam/sdk/tests/testfiles/ur5e-real.urdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 8184f848e..cf16c9ba2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -501,7 +501,16 @@ FetchContent_Declare( FIND_PACKAGE_ARGS ${VIAMCPPSDK_XTENSOR_VERSION_MINIMUM} CONFIG ) -FetchContent_MakeAvailable(xtl xtensor) +# No version arg on FIND_PACKAGE_ARGS: Eigen's SameMajorVersion check +# rejects newer majors (4+/5+) from conan-center; we use only stable 3+ API. +FetchContent_Declare( + Eigen3 + GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git + GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c # 3.4.0 + FIND_PACKAGE_ARGS CONFIG +) + +FetchContent_MakeAvailable(xtl xtensor Eigen3) # Pull in our subdirectories add_subdirectory(src/viam) diff --git a/conanfile.py b/conanfile.py index 0c6d91015..3d94f092a 100644 --- a/conanfile.py +++ b/conanfile.py @@ -67,6 +67,7 @@ def requirements(self): self.requires(self._grpc_requires()) self.requires('protobuf/[>=3.17.1 <6.30.0]') self.requires(self._xtensor_requires(), transitive_headers=True) + self.requires('eigen/[>=3.3.0]', transitive_headers=True) def build_requirements(self): if self.options.offline_proto_generation: @@ -150,6 +151,7 @@ def package_info(self): "grpc::grpc++_reflection", "protobuf::libprotobuf", "xtensor::xtensor", + "eigen::eigen", "viamapi", ]) diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index b5b129449..656e5dd71 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -136,6 +136,7 @@ target_sources(viamsdk module/service.cpp module/private/data_consumer_query.cpp referenceframe/frame.cpp + referenceframe/urdf_model_table.cpp registry/registry.cpp resource/reconfigurable.cpp resource/resource.cpp @@ -217,6 +218,7 @@ target_sources(viamsdk ../../viam/sdk/module/service.hpp ../../viam/sdk/module/signal_manager.hpp ../../viam/sdk/referenceframe/frame.hpp + ../../viam/sdk/referenceframe/urdf_model_table.hpp ../../viam/sdk/registry/registry.hpp ../../viam/sdk/resource/reconfigurable.hpp ../../viam/sdk/resource/resource.hpp @@ -301,6 +303,7 @@ target_link_libraries(viamsdk PUBLIC Boost::log PUBLIC Boost::log_setup PUBLIC xtensor + PUBLIC Eigen3::Eigen PRIVATE ${VIAMCPPSDK_GRPCXX_REFLECTION_LIBRARIES} PRIVATE ${VIAMCPPSDK_GRPCXX_LIBRARIES} PRIVATE ${VIAMCPPSDK_GRPC_LIBRARIES} diff --git a/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp b/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp new file mode 100644 index 000000000..2bb669103 --- /dev/null +++ b/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp @@ -0,0 +1,43 @@ +/// @file referenceframe/private/urdf_model_table_internals.hpp +/// @brief Internal helpers for urdf_to_model_table. Exposed only to +/// the testsuite; not a stable public API. +#pragma once + +#include +#include + +#include +#include + +#include +#include + +namespace viam { +namespace sdk { +namespace urdf_model_table_internals { + +/// @brief Per-joint parse result, pre-validation. +struct ParsedJoint { + std::string name; + std::string type_str; ///< raw URDF type attribute + std::string parent_link; + std::string child_link; + Eigen::Vector3d xyz{0, 0, 0}; + Eigen::Vector3d rpy{0, 0, 0}; + boost::optional axis_opt; +}; + +/// @brief Parse all elements from a URDF, no topology checks. +std::vector parse_urdf(const KinematicsDataURDF& urdf); + +/// @brief Order joints root -> leaf along the kinematic chain. +/// @throws viam::sdk::Exception on multi-root, branching, or disconnect. +std::vector walk_urdf_chain(const std::vector& joints); + +/// @brief Convert one validated ParsedJoint to a JointRow. +/// @throws viam::sdk::Exception on unsupported type or zero non-fixed axis. +JointRow to_row(const ParsedJoint& parsed); + +} // namespace urdf_model_table_internals +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/urdf_model_table.cpp b/src/viam/sdk/referenceframe/urdf_model_table.cpp new file mode 100644 index 000000000..1883aeaa5 --- /dev/null +++ b/src/viam/sdk/referenceframe/urdf_model_table.cpp @@ -0,0 +1,251 @@ +#include + +#include +#include +#include + +#include +#include + +#if defined(__has_include) && (__has_include()) +#include +#else +#include +#endif +#if defined(__has_include) && (__has_include()) +#include +#else +#include +#endif + +#include +#include + +namespace viam { +namespace sdk { +namespace urdf_model_table_internals { + +namespace { + +Eigen::Vector3d parse_triple(const std::string& s) { + std::istringstream iss(s); + double x = 0, y = 0, z = 0; + if (!(iss >> x >> y >> z)) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: failed to parse space-delimited triple: '" + s + "'"); + } + return Eigen::Vector3d(x, y, z); +} + +} // namespace + +std::vector parse_urdf(const KinematicsDataURDF& urdf) { + namespace pt = boost::property_tree; + pt::ptree tree; + std::string text(reinterpret_cast(urdf.bytes.data()), urdf.bytes.size()); + std::istringstream iss(text); + try { + pt::read_xml(iss, tree); + } catch (const pt::xml_parser_error& e) { + throw Exception(ErrorCondition::k_general, + std::string("URDFToModelTable: failed to parse URDF XML: ") + e.what()); + } + + const auto robot_it = tree.find("robot"); + if (robot_it == tree.not_found()) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: no root element in URDF"); + } + const pt::ptree& robot = robot_it->second; + + std::vector joints; + for (const auto& child : robot) { + if (child.first != "joint") + continue; + const pt::ptree& jnode = child.second; + + ParsedJoint j; + j.name = jnode.get(".name", ""); + j.type_str = jnode.get(".type", ""); + j.parent_link = jnode.get("parent..link", ""); + j.child_link = jnode.get("child..link", ""); + + if (j.type_str.empty() || j.parent_link.empty() || j.child_link.empty()) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: joint '" + j.name + + "' missing required attribute (type/parent/child)"); + } + + const auto origin_opt = jnode.get_child_optional("origin"); + if (origin_opt) { + j.xyz = parse_triple(origin_opt->get(".xyz", "0 0 0")); + j.rpy = parse_triple(origin_opt->get(".rpy", "0 0 0")); + } + const auto axis_opt_node = jnode.get_child_optional("axis"); + if (axis_opt_node) { + j.axis_opt = parse_triple(axis_opt_node->get(".xyz", "1 0 0")); + } + joints.push_back(std::move(j)); + } + return joints; +} + +std::vector walk_urdf_chain(const std::vector& joints) { + std::map> by_parent; + std::set all_parents, all_children; + for (const auto& j : joints) { + by_parent[j.parent_link].push_back(&j); + all_parents.insert(j.parent_link); + all_children.insert(j.child_link); + } + + std::vector roots; + for (const auto& p : all_parents) { + if (all_children.find(p) == all_children.end()) + roots.push_back(p); + } + if (roots.size() != 1) { + std::string list; + for (const auto& r : roots) + list += "'" + r + "' "; + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: expected exactly one root link, found " + + std::to_string(roots.size()) + ": " + list); + } + + std::vector ordered; + ordered.reserve(joints.size()); + std::set visited; + std::string current = roots[0]; + while (true) { + if (!visited.insert(current).second) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: cycle in URDF chain at link '" + current + "'"); + } + auto it = by_parent.find(current); + if (it == by_parent.end() || it->second.empty()) + break; + if (it->second.size() > 1) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: branching topology at link '" + current + "' (" + + std::to_string(it->second.size()) + " outgoing joints)"); + } + const ParsedJoint* j = it->second.front(); + ordered.push_back(*j); + current = j->child_link; + } + if (ordered.size() != joints.size()) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: chain walk visited " + std::to_string(ordered.size()) + + " of " + std::to_string(joints.size()) + " joints"); + } + return ordered; +} + +JointRow to_row(const ParsedJoint& parsed) { + JointRow row; + row.name = parsed.name; + row.xyz = parsed.xyz; + row.rpy = parsed.rpy; + + if (parsed.type_str == "revolute") + row.type = JointType::revolute; + else if (parsed.type_str == "continuous") + row.type = JointType::continuous; + else if (parsed.type_str == "prismatic") + row.type = JointType::prismatic; + else if (parsed.type_str == "fixed") + row.type = JointType::fixed; + else { + throw Exception(ErrorCondition::k_not_supported, + "URDFToModelTable: joint '" + parsed.name + "' has unsupported type '" + + parsed.type_str + + "' (supported: revolute, continuous, prismatic, fixed)"); + } + + if (row.type == JointType::fixed) { + row.axis = Eigen::Vector3d(0, 0, 0); + } else if (parsed.axis_opt) { + if (parsed.axis_opt->norm() < 1e-12) { + throw Exception( + ErrorCondition::k_general, + "URDFToModelTable: joint '" + parsed.name + "' has zero-magnitude axis"); + } + row.axis = *parsed.axis_opt; + } else { + row.axis = Eigen::Vector3d(1, 0, 0); // URDF default + } + return row; +} + +} // namespace urdf_model_table_internals + +ModelTable urdf_to_model_table(const KinematicsDataURDF& urdf) { + using namespace urdf_model_table_internals; + auto chain = walk_urdf_chain(parse_urdf(urdf)); + ModelTable table; + table.reserve(chain.size()); + for (const auto& j : chain) { + table.push_back(to_row(j)); + } + return table; +} + +xt::xarray model_table_to_tensor(const ModelTable& table) { + if (table.empty()) { + throw Exception(ErrorCondition::k_general, "model_table_to_tensor: empty model table"); + } + xt::xarray out = xt::zeros({table.size(), std::size_t{10}}); + for (std::size_t i = 0; i < table.size(); ++i) { + const auto& r = table[i]; + out(i, 0) = r.xyz.x(); + out(i, 1) = r.xyz.y(); + out(i, 2) = r.xyz.z(); + out(i, 3) = r.rpy.x(); + out(i, 4) = r.rpy.y(); + out(i, 5) = r.rpy.z(); + out(i, 6) = r.axis.x(); + out(i, 7) = r.axis.y(); + out(i, 8) = r.axis.z(); + out(i, 9) = static_cast(static_cast(r.type)); + } + return out; +} + +ModelTable tensor_to_model_table(const xt::xarray& tensor) { + if (tensor.dimension() != 2) { + throw Exception(ErrorCondition::k_general, + "tensor_to_model_table: expected 2D tensor, got " + + std::to_string(tensor.dimension()) + "D"); + } + if (tensor.shape()[1] != 10) { + throw Exception(ErrorCondition::k_general, + "tensor_to_model_table: expected shape (n, 10), got (n, " + + std::to_string(tensor.shape()[1]) + ")"); + } + if (tensor.shape()[0] == 0) { + throw Exception(ErrorCondition::k_general, "tensor_to_model_table: empty tensor"); + } + ModelTable table; + table.reserve(tensor.shape()[0]); + for (std::size_t i = 0; i < tensor.shape()[0]; ++i) { + const double type_v = tensor(i, 9); + const int type_i = static_cast(type_v); + if (static_cast(type_i) != type_v || type_i < 0 || type_i > 3) { + throw Exception(ErrorCondition::k_general, + "tensor_to_model_table: row " + std::to_string(i) + + " has invalid joint type value " + std::to_string(type_v) + + " (expected integer in [0, 3])"); + } + JointRow row; + row.xyz = Eigen::Vector3d(tensor(i, 0), tensor(i, 1), tensor(i, 2)); + row.rpy = Eigen::Vector3d(tensor(i, 3), tensor(i, 4), tensor(i, 5)); + row.axis = Eigen::Vector3d(tensor(i, 6), tensor(i, 7), tensor(i, 8)); + row.type = static_cast(type_i); + table.push_back(std::move(row)); + } + return table; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/urdf_model_table.hpp b/src/viam/sdk/referenceframe/urdf_model_table.hpp new file mode 100644 index 000000000..44c43a091 --- /dev/null +++ b/src/viam/sdk/referenceframe/urdf_model_table.hpp @@ -0,0 +1,56 @@ +/// @file referenceframe/urdf_model_table.hpp +/// @brief Convert a serial URDF chain to a typed per-joint model table +/// and from the table to a double (n, 10) tensor. +#pragma once + +#include +#include + +#include +#if defined(__has_include) && (__has_include()) +#include +#else +#include +#endif + +#include + +namespace viam { +namespace sdk { + +/// @brief URDF joint type, restricted to arm-relevant joints. +enum class JointType { revolute, continuous, prismatic, fixed }; + +/// @brief One row of the model table: the per-joint URDF fields. +/// @note `xyz`/`rpy`/`axis` are taken directly from the URDF. +struct JointRow { + std::string name; + Eigen::Vector3d xyz{0, 0, 0}; + Eigen::Vector3d rpy{0, 0, 0}; + Eigen::Vector3d axis{0, 0, 0}; + JointType type = JointType::fixed; +}; + +using ModelTable = std::vector; + +/// @brief Parse a URDF and return the per-joint model table in chain order. +/// @throws viam::sdk::Exception on parse error, branching / multi-root / +/// disconnected topology, unsupported joint types, or zero axis. +ModelTable urdf_to_model_table(const KinematicsDataURDF& urdf); + +/// @brief Convert a model table to a double tensor of shape (n, 10). +/// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type as +/// underlying enum value (0=revolute, 1=continuous, 2=prismatic, 3=fixed). +/// @throws viam::sdk::Exception on empty input. +xt::xarray model_table_to_tensor(const ModelTable& table); + +/// @brief Inverse of model_table_to_tensor: rehydrate an (n, 10) double +/// tensor into a model table. Reconstructed rows have empty `name` +/// (names are not carried in the tensor). +/// @throws viam::sdk::Exception on non-2D input, wrong column count, +/// empty input, or invalid joint-type encoding (col 9 must be an +/// integer in [0, 3]). +ModelTable tensor_to_model_table(const xt::xarray& tensor); + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index fd0f27006..f79854f21 100644 --- a/src/viam/sdk/tests/CMakeLists.txt +++ b/src/viam/sdk/tests/CMakeLists.txt @@ -85,3 +85,9 @@ viamcppsdk_add_boost_test(test_sensor.cpp) viamcppsdk_add_boost_test(test_servo.cpp) viamcppsdk_add_boost_test(test_switch.cpp) viamcppsdk_add_boost_test(test_robot.cpp) +viamcppsdk_add_boost_test(test_urdf_model_table.cpp) + +target_compile_definitions(test_urdf_model_table + PRIVATE + VIAMCPPSDK_TEST_TESTFILES_DIR="${CMAKE_CURRENT_SOURCE_DIR}/testfiles" +) diff --git a/src/viam/sdk/tests/test_urdf_model_table.cpp b/src/viam/sdk/tests/test_urdf_model_table.cpp new file mode 100644 index 000000000..c1925d950 --- /dev/null +++ b/src/viam/sdk/tests/test_urdf_model_table.cpp @@ -0,0 +1,523 @@ +#define BOOST_TEST_MODULE test module test_urdf_model_table + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace viam::sdk; +using namespace viam::sdk::urdf_model_table_internals; + +namespace { +KinematicsDataURDF urdf_from_string(const std::string& xml) { + std::vector bytes(xml.begin(), xml.end()); + return KinematicsDataURDF(std::move(bytes)); +} +} // namespace + +BOOST_AUTO_TEST_CASE(parse_urdf_minimal) { + const std::string xml = R"( + + + + + + + + + +)"; + auto parsed = parse_urdf(urdf_from_string(xml)); + BOOST_REQUIRE_EQUAL(parsed.size(), 1u); + BOOST_CHECK_EQUAL(parsed[0].name, "j1"); + BOOST_CHECK_EQUAL(parsed[0].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[0].parent_link, "base"); + BOOST_CHECK_EQUAL(parsed[0].child_link, "l1"); + BOOST_CHECK_CLOSE(parsed[0].xyz.x(), 1.0, 1e-9); + BOOST_CHECK_CLOSE(parsed[0].xyz.z(), 3.0, 1e-9); + BOOST_CHECK_CLOSE(parsed[0].rpy.y(), 0.2, 1e-9); + BOOST_REQUIRE(parsed[0].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[0].axis_opt->z(), 1.0, 1e-9); +} + +BOOST_AUTO_TEST_CASE(parse_urdf_missing_origin) { + const std::string xml = R"( + + + +)"; + auto parsed = parse_urdf(urdf_from_string(xml)); + BOOST_REQUIRE_EQUAL(parsed.size(), 1u); + BOOST_CHECK(parsed[0].xyz.isZero()); + BOOST_CHECK(parsed[0].rpy.isZero()); +} + +BOOST_AUTO_TEST_CASE(parse_urdf_missing_axis) { + const std::string xml = R"( + + + +)"; + auto parsed = parse_urdf(urdf_from_string(xml)); + BOOST_REQUIRE_EQUAL(parsed.size(), 1u); + BOOST_CHECK(!parsed[0].axis_opt.has_value()); +} + +BOOST_AUTO_TEST_CASE(parse_urdf_malformed_xml) { + auto match = [](const Exception& e) { + return std::string(e.what()).find("failed to parse URDF XML") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(parse_urdf(urdf_from_string(">")), Exception, match); +} + +BOOST_AUTO_TEST_CASE(parse_urdf_no_robot_root) { + const std::string xml = R"()"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("no root element") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(parse_urdf(urdf_from_string(xml)), Exception, match); +} + +BOOST_AUTO_TEST_CASE(parse_urdf_missing_required_attribute_throws) { + const std::string xml = R"( + + +)"; + auto match = [](const Exception& e) { + return std::string(e.what()).find("missing required attribute") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(parse_urdf(urdf_from_string(xml)), Exception, match); +} + +namespace { +ParsedJoint mk(const std::string& name, + const std::string& parent, + const std::string& child, + const std::string& type = "revolute") { + ParsedJoint j; + j.name = name; + j.type_str = type; + j.parent_link = parent; + j.child_link = child; + return j; +} +} // namespace + +BOOST_AUTO_TEST_CASE(walk_urdf_chain_serial_ok) { + std::vector in{mk("j2", "l1", "l2"), mk("j1", "base", "l1"), mk("j3", "l2", "l3")}; + auto out = walk_urdf_chain(in); + BOOST_REQUIRE_EQUAL(out.size(), 3u); + BOOST_CHECK_EQUAL(out[0].name, "j1"); + BOOST_CHECK_EQUAL(out[1].name, "j2"); + BOOST_CHECK_EQUAL(out[2].name, "j3"); +} + +BOOST_AUTO_TEST_CASE(walk_urdf_chain_branching_throws) { + std::vector in{ + mk("j1", "base", "l1"), mk("j2a", "l1", "l2"), mk("j2b", "l1", "l3")}; + auto match = [](const Exception& e) { + return std::string(e.what()).find("branching topology") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(walk_urdf_chain(in), Exception, match); +} + +BOOST_AUTO_TEST_CASE(walk_urdf_chain_multiple_roots_throws) { + std::vector in{mk("j1", "baseA", "l1"), mk("j2", "baseB", "l2")}; + auto match = [](const Exception& e) { + return std::string(e.what()).find("expected exactly one root link") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(walk_urdf_chain(in), Exception, match); +} + +BOOST_AUTO_TEST_CASE(walk_urdf_chain_disconnected_throws) { + std::vector in{ + mk("j1", "base", "l1"), mk("j2", "l1", "l2"), mk("j3", "floatA", "floatB")}; + auto match = [](const Exception& e) { + return std::string(e.what()).find("expected exactly one root link") != std::string::npos || + std::string(e.what()).find("chain walk visited") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(walk_urdf_chain(in), Exception, match); +} + +BOOST_AUTO_TEST_CASE(walk_urdf_chain_cycle_throws) { + // base -> A -> B -> A (back-edge from B to A creates a cycle reachable from + // the valid root `base`). + std::vector in{mk("j1", "base", "A"), mk("j2", "A", "B"), mk("j3", "B", "A")}; + auto match = [](const Exception& e) { + return std::string(e.what()).find("cycle in URDF chain") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(walk_urdf_chain(in), Exception, match); +} + +BOOST_AUTO_TEST_CASE(to_row_each_type) { + const std::vector> cases{ + {"revolute", JointType::revolute}, + {"continuous", JointType::continuous}, + {"prismatic", JointType::prismatic}, + {"fixed", JointType::fixed}, + }; + for (const auto& c : cases) { + ParsedJoint p = mk("j", "a", "b", c.first); + p.axis_opt = Eigen::Vector3d(0, 0, 1); + BOOST_CHECK(to_row(p).type == c.second); + } +} + +BOOST_AUTO_TEST_CASE(to_row_fixed_zeroes_axis) { + ParsedJoint p = mk("j", "a", "b", "fixed"); + p.axis_opt = Eigen::Vector3d(1, 2, 3); + auto row = to_row(p); + BOOST_CHECK(row.axis.isZero()); +} + +BOOST_AUTO_TEST_CASE(to_row_default_axis_for_nonfixed) { + ParsedJoint p = mk("j", "a", "b", "revolute"); // no axis_opt + auto row = to_row(p); + BOOST_CHECK_CLOSE(row.axis.x(), 1.0, 1e-9); + BOOST_CHECK_SMALL(row.axis.y(), 1e-9); + BOOST_CHECK_SMALL(row.axis.z(), 1e-9); +} + +BOOST_AUTO_TEST_CASE(to_row_unsupported_type_throws) { + ParsedJoint p = mk("jf", "a", "b", "floating"); + auto match = [](const Exception& e) { + return std::string(e.what()).find("unsupported type 'floating'") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(to_row(p), Exception, match); +} + +BOOST_AUTO_TEST_CASE(to_row_zero_axis_throws) { + ParsedJoint p = mk("j", "a", "b", "revolute"); + p.axis_opt = Eigen::Vector3d(0, 0, 0); + auto match = [](const Exception& e) { + return std::string(e.what()).find("zero-magnitude axis") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(to_row(p), Exception, match); +} + +BOOST_AUTO_TEST_CASE(urdf_to_model_table_end_to_end_inline) { + const std::string xml = R"( + + + + + + + + + + +)"; + auto table = urdf_to_model_table(urdf_from_string(xml)); + BOOST_REQUIRE_EQUAL(table.size(), 2u); + + BOOST_CHECK_EQUAL(table[0].name, "j1"); + BOOST_CHECK(table[0].type == JointType::revolute); + BOOST_CHECK_CLOSE(table[0].xyz.z(), 0.5, 1e-9); + BOOST_CHECK_CLOSE(table[0].axis.z(), 1.0, 1e-9); + + BOOST_CHECK_EQUAL(table[1].name, "fix"); + BOOST_CHECK(table[1].type == JointType::fixed); + BOOST_CHECK(table[1].axis.isZero()); +} + +BOOST_AUTO_TEST_CASE(tensor_shape_and_dtype) { + std::vector table{ + JointRow{"a", {1, 2, 3}, {0.1, 0.2, 0.3}, {0, 0, 1}, JointType::revolute}, + JointRow{"b", {4, 5, 6}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + }; + auto t = model_table_to_tensor(table); + BOOST_CHECK_EQUAL(t.dimension(), 2u); + BOOST_CHECK_EQUAL(t.shape()[0], 2u); + BOOST_CHECK_EQUAL(t.shape()[1], 10u); + static_assert(std::is_same::value, "dtype must be double"); +} + +BOOST_AUTO_TEST_CASE(tensor_geometry_columns) { + std::vector table{ + JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::revolute}, + }; + auto t = model_table_to_tensor(table); + BOOST_CHECK_CLOSE(t(0, 0), 1.5, 1e-9); + BOOST_CHECK_CLOSE(t(0, 1), 2.5, 1e-9); + BOOST_CHECK_CLOSE(t(0, 2), 3.5, 1e-9); + BOOST_CHECK_CLOSE(t(0, 3), 0.1, 1e-9); + BOOST_CHECK_CLOSE(t(0, 6), 0.4, 1e-9); + BOOST_CHECK_CLOSE(t(0, 8), 0.6, 1e-9); +} + +BOOST_AUTO_TEST_CASE(tensor_type_column) { + const Eigen::Vector3d z{0, 0, 0}; // explicit zero; Eigen{} default-ctor is uninitialized + std::vector table{ + JointRow{"a", z, z, {0, 0, 1}, JointType::revolute}, + JointRow{"b", z, z, {0, 0, 1}, JointType::continuous}, + JointRow{"c", z, z, {1, 0, 0}, JointType::prismatic}, + JointRow{"d", z, z, {0, 0, 0}, JointType::fixed}, + }; + auto t = model_table_to_tensor(table); + BOOST_CHECK_EQUAL(t(0, 9), 0.0); // revolute + BOOST_CHECK_EQUAL(t(1, 9), 1.0); // continuous + BOOST_CHECK_EQUAL(t(2, 9), 2.0); // prismatic + BOOST_CHECK_EQUAL(t(3, 9), 3.0); // fixed +} + +BOOST_AUTO_TEST_CASE(tensor_fixed_joint_axis_zero) { + std::vector table{ + JointRow{"f", {0.1, 0.2, 0.3}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + }; + auto t = model_table_to_tensor(table); + BOOST_CHECK_EQUAL(t(0, 6), 0.0); + BOOST_CHECK_EQUAL(t(0, 7), 0.0); + BOOST_CHECK_EQUAL(t(0, 8), 0.0); + BOOST_CHECK_EQUAL(t(0, 9), 3.0); // fixed encoded as 3 +} + +BOOST_AUTO_TEST_CASE(model_table_to_tensor_empty_throws) { + auto match = [](const Exception& e) { + return std::string(e.what()).find("empty model table") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(model_table_to_tensor({}), Exception, match); +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_round_trip) { + std::vector table{ + JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::revolute}, + JointRow{"b", {4, 5, 6}, {0, 0, 0}, {1, 0, 0}, JointType::prismatic}, + JointRow{"c", {7, 8, 9}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + }; + auto t = model_table_to_tensor(table); + auto rebuilt = tensor_to_model_table(t); + + BOOST_REQUIRE_EQUAL(rebuilt.size(), table.size()); + for (std::size_t i = 0; i < table.size(); ++i) { + BOOST_CHECK(rebuilt[i].xyz == table[i].xyz); + BOOST_CHECK(rebuilt[i].rpy == table[i].rpy); + BOOST_CHECK(rebuilt[i].axis == table[i].axis); + BOOST_CHECK(rebuilt[i].type == table[i].type); + BOOST_CHECK(rebuilt[i].name.empty()); // names are not carried in the tensor + } +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_wrong_dim_throws) { + xt::xarray t1d = xt::zeros({std::size_t{10}}); + auto match = [](const Exception& e) { + return std::string(e.what()).find("expected 2D tensor") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(tensor_to_model_table(t1d), Exception, match); +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_wrong_cols_throws) { + xt::xarray bad = xt::zeros({std::size_t{3}, std::size_t{5}}); + auto match = [](const Exception& e) { + return std::string(e.what()).find("expected shape (n, 10)") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(tensor_to_model_table(bad), Exception, match); +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_empty_throws) { + xt::xarray empty = xt::zeros({std::size_t{0}, std::size_t{10}}); + auto match = [](const Exception& e) { + return std::string(e.what()).find("empty tensor") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(tensor_to_model_table(empty), Exception, match); +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_fractional_type_throws) { + xt::xarray t = xt::zeros({std::size_t{1}, std::size_t{10}}); + t(0, 9) = 0.5; // non-integer joint type + auto match = [](const Exception& e) { + return std::string(e.what()).find("invalid joint type value") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(tensor_to_model_table(t), Exception, match); +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_out_of_range_type_throws) { + xt::xarray t = xt::zeros({std::size_t{1}, std::size_t{10}}); + t(0, 9) = 7.0; // outside [0, 3] + auto match = [](const Exception& e) { + return std::string(e.what()).find("invalid joint type value") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(tensor_to_model_table(t), Exception, match); +} + +namespace { +KinematicsDataURDF load_urdf(const std::string& filename) { + std::ifstream f(std::string(VIAMCPPSDK_TEST_TESTFILES_DIR) + "/" + filename, std::ios::binary); + if (!f) + throw std::runtime_error("failed to open " + filename); + std::ostringstream buf; + buf << f.rdbuf(); + return urdf_from_string(buf.str()); +} +} // namespace + +BOOST_AUTO_TEST_CASE(model_table_gp12_has_fixed_tool0) { + auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + bool has_fixed_tool = false; + for (const auto& row : table) { + if (row.type == JointType::fixed) { + has_fixed_tool = true; + BOOST_CHECK(row.axis.isZero()); + } + } + BOOST_CHECK(has_fixed_tool); +} + +BOOST_AUTO_TEST_CASE(tensor_round_trip_gp12) { + auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto t = model_table_to_tensor(table); + BOOST_CHECK_EQUAL(t.shape()[0], table.size()); + BOOST_CHECK_EQUAL(t.shape()[1], 10u); + for (std::size_t i = 0; i < table.size(); ++i) { + const double type_v = t(i, 9); + BOOST_CHECK(type_v >= 0.0 && type_v <= 3.0); + BOOST_CHECK_EQUAL(type_v, static_cast(static_cast(type_v))); + } +} + +BOOST_AUTO_TEST_CASE(model_table_gp12_full_chain) { + auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + + // 6 revolute joints + 1 fixed tool joint. + BOOST_REQUIRE_EQUAL(table.size(), 7u); + + // Chain order: 6 revolute joints in URDF order, then the fixed tool joint. + BOOST_CHECK_EQUAL(table[0].name, "joint_1_s"); + BOOST_CHECK_EQUAL(table[1].name, "joint_2_l"); + BOOST_CHECK_EQUAL(table[2].name, "joint_3_u"); + BOOST_CHECK_EQUAL(table[3].name, "joint_4_r"); + BOOST_CHECK_EQUAL(table[4].name, "joint_5_b"); + BOOST_CHECK_EQUAL(table[5].name, "joint_6_t"); + BOOST_CHECK_EQUAL(table[6].name, "joint_6_t-tool0"); + + // Types: 6 revolute then 1 fixed. + for (std::size_t i = 0; i < 6; ++i) { + BOOST_CHECK(table[i].type == JointType::revolute); + } + BOOST_CHECK(table[6].type == JointType::fixed); + + // Joint 1: shoulder, vertical axis at z=0.450. + BOOST_CHECK_SMALL(table[0].xyz.x(), 1e-9); + BOOST_CHECK_SMALL(table[0].xyz.y(), 1e-9); + BOOST_CHECK_CLOSE(table[0].xyz.z(), 0.450, 1e-9); + BOOST_CHECK_SMALL(table[0].axis.x(), 1e-9); + BOOST_CHECK_SMALL(table[0].axis.y(), 1e-9); + BOOST_CHECK_CLOSE(table[0].axis.z(), 1.0, 1e-9); + + // Joint 4: elbow offset (0.640, 0, 0.200), axis=(-1, 0, 0). + BOOST_CHECK_CLOSE(table[3].xyz.x(), 0.640, 1e-9); + BOOST_CHECK_CLOSE(table[3].xyz.z(), 0.200, 1e-9); + BOOST_CHECK_CLOSE(table[3].axis.x(), -1.0, 1e-9); + + // Fixed tool joint: nonzero rpy, zero axis (substituted by to_row). + BOOST_CHECK_CLOSE(table[6].xyz.x(), 0.100, 1e-9); + BOOST_CHECK_CLOSE(table[6].rpy.y(), -1.570796, 1e-3); + BOOST_CHECK(table[6].axis.isZero()); + + // Tensor cross-check: same 7 rows, 10 columns, type column at index 9. + auto t = model_table_to_tensor(table); + BOOST_REQUIRE_EQUAL(t.shape()[0], 7u); + BOOST_REQUIRE_EQUAL(t.shape()[1], 10u); + BOOST_CHECK_EQUAL(t(6, 9), 3.0); // tool0 is fixed (enum value 3) + BOOST_CHECK_EQUAL(t(0, 9), 0.0); // joint_1_s is revolute (enum value 0) + BOOST_CHECK_CLOSE(t(0, 2), 0.450, 1e-9); // joint_1_s z +} + +BOOST_AUTO_TEST_CASE(parse_urdf_gp12_full_file) { + auto parsed = parse_urdf(load_urdf("gp12.urdf")); + + BOOST_REQUIRE_EQUAL(parsed.size(), 7u); + + // Joint 0: joint_1_s + BOOST_CHECK_EQUAL(parsed[0].name, "joint_1_s"); + BOOST_CHECK_EQUAL(parsed[0].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[0].parent_link, "base_link"); + BOOST_CHECK_EQUAL(parsed[0].child_link, "link_1_s"); + BOOST_CHECK_SMALL(parsed[0].xyz.x(), 1e-9); + BOOST_CHECK_SMALL(parsed[0].xyz.y(), 1e-9); + BOOST_CHECK_CLOSE(parsed[0].xyz.z(), 0.450, 1e-9); + BOOST_CHECK(parsed[0].rpy.isZero()); + BOOST_REQUIRE(parsed[0].axis_opt.has_value()); + BOOST_CHECK_SMALL(parsed[0].axis_opt->x(), 1e-9); + BOOST_CHECK_SMALL(parsed[0].axis_opt->y(), 1e-9); + BOOST_CHECK_CLOSE(parsed[0].axis_opt->z(), 1.0, 1e-9); + + // Joint 1: joint_2_l + BOOST_CHECK_EQUAL(parsed[1].name, "joint_2_l"); + BOOST_CHECK_EQUAL(parsed[1].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[1].parent_link, "link_1_s"); + BOOST_CHECK_EQUAL(parsed[1].child_link, "link_2_l"); + BOOST_CHECK_CLOSE(parsed[1].xyz.x(), 0.155, 1e-9); + BOOST_CHECK_SMALL(parsed[1].xyz.y(), 1e-9); + BOOST_CHECK_SMALL(parsed[1].xyz.z(), 1e-9); + BOOST_REQUIRE(parsed[1].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[1].axis_opt->y(), 1.0, 1e-9); + + // Joint 2: joint_3_u + BOOST_CHECK_EQUAL(parsed[2].name, "joint_3_u"); + BOOST_CHECK_EQUAL(parsed[2].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[2].parent_link, "link_2_l"); + BOOST_CHECK_EQUAL(parsed[2].child_link, "link_3_u"); + BOOST_CHECK_CLOSE(parsed[2].xyz.z(), 0.614, 1e-9); + BOOST_REQUIRE(parsed[2].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[2].axis_opt->y(), -1.0, 1e-9); + + // Joint 3: joint_4_r + BOOST_CHECK_EQUAL(parsed[3].name, "joint_4_r"); + BOOST_CHECK_EQUAL(parsed[3].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[3].parent_link, "link_3_u"); + BOOST_CHECK_EQUAL(parsed[3].child_link, "link_4_r"); + BOOST_CHECK_CLOSE(parsed[3].xyz.x(), 0.640, 1e-9); + BOOST_CHECK_CLOSE(parsed[3].xyz.z(), 0.200, 1e-9); + BOOST_REQUIRE(parsed[3].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[3].axis_opt->x(), -1.0, 1e-9); + + // Joint 4: joint_5_b + BOOST_CHECK_EQUAL(parsed[4].name, "joint_5_b"); + BOOST_CHECK_EQUAL(parsed[4].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[4].parent_link, "link_4_r"); + BOOST_CHECK_EQUAL(parsed[4].child_link, "link_5_b"); + BOOST_CHECK(parsed[4].xyz.isZero()); + BOOST_REQUIRE(parsed[4].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[4].axis_opt->y(), -1.0, 1e-9); + + // Joint 5: joint_6_t + BOOST_CHECK_EQUAL(parsed[5].name, "joint_6_t"); + BOOST_CHECK_EQUAL(parsed[5].type_str, "revolute"); + BOOST_CHECK_EQUAL(parsed[5].parent_link, "link_5_b"); + BOOST_CHECK_EQUAL(parsed[5].child_link, "link_6_t"); + BOOST_CHECK(parsed[5].xyz.isZero()); + BOOST_REQUIRE(parsed[5].axis_opt.has_value()); + BOOST_CHECK_CLOSE(parsed[5].axis_opt->x(), -1.0, 1e-9); + + // Joint 6: joint_6_t-tool0 (fixed, no element) + BOOST_CHECK_EQUAL(parsed[6].name, "joint_6_t-tool0"); + BOOST_CHECK_EQUAL(parsed[6].type_str, "fixed"); + BOOST_CHECK_EQUAL(parsed[6].parent_link, "link_6_t"); + BOOST_CHECK_EQUAL(parsed[6].child_link, "tool0"); + BOOST_CHECK_CLOSE(parsed[6].xyz.x(), 0.100, 1e-9); + BOOST_CHECK_SMALL(parsed[6].xyz.y(), 1e-9); + BOOST_CHECK_SMALL(parsed[6].xyz.z(), 1e-9); + BOOST_CHECK_CLOSE(parsed[6].rpy.x(), 3.1415926, 1e-3); + BOOST_CHECK_CLOSE(parsed[6].rpy.y(), -1.570796, 1e-3); + BOOST_CHECK_SMALL(parsed[6].rpy.z(), 1e-9); + BOOST_CHECK(!parsed[6].axis_opt.has_value()); // fixed joint has no in URDF +} + +BOOST_AUTO_TEST_CASE(tensor_to_model_table_gp12_round_trip) { + auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto t = model_table_to_tensor(table); + auto rebuilt = tensor_to_model_table(t); + BOOST_REQUIRE_EQUAL(rebuilt.size(), table.size()); + for (std::size_t i = 0; i < table.size(); ++i) { + BOOST_CHECK(rebuilt[i].xyz == table[i].xyz); + BOOST_CHECK(rebuilt[i].rpy == table[i].rpy); + BOOST_CHECK(rebuilt[i].axis == table[i].axis); + BOOST_CHECK(rebuilt[i].type == table[i].type); + } +} diff --git a/src/viam/sdk/tests/testfiles/gp12.urdf b/src/viam/sdk/tests/testfiles/gp12.urdf new file mode 100644 index 000000000..a8f2bbd36 --- /dev/null +++ b/src/viam/sdk/tests/testfiles/gp12.urdf @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/viam/sdk/tests/testfiles/ur5e-real.urdf b/src/viam/sdk/tests/testfiles/ur5e-real.urdf new file mode 100644 index 000000000..9f71d808f --- /dev/null +++ b/src/viam/sdk/tests/testfiles/ur5e-real.urdf @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 79a4d089e2eb36a0eba1aba57f387e5dd65b9c35 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 26 May 2026 11:14:21 -0400 Subject: [PATCH 02/10] no more eigen --- CMakeLists.txt | 11 +------- src/viam/sdk/CMakeLists.txt | 1 - src/viam/sdk/common/linear_algebra.hpp | 7 +++++ .../private/urdf_model_table_internals.hpp | 8 +++--- .../sdk/referenceframe/urdf_model_table.cpp | 21 +++++++++------ .../sdk/referenceframe/urdf_model_table.hpp | 10 +++---- src/viam/sdk/tests/test_urdf_model_table.cpp | 26 +++++++++---------- 7 files changed, 43 insertions(+), 41 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9559a707b..d2b8c8803 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -514,16 +514,7 @@ FetchContent_Declare( FIND_PACKAGE_ARGS ${VIAMCPPSDK_XTENSOR_VERSION_MINIMUM} CONFIG ) -# No version arg on FIND_PACKAGE_ARGS: Eigen's SameMajorVersion check -# rejects newer majors (4+/5+) from conan-center; we use only stable 3+ API. -FetchContent_Declare( - Eigen3 - GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git - GIT_TAG 3147391d946bb4b6c68edd901f2add6ac1f31f8c # 3.4.0 - FIND_PACKAGE_ARGS CONFIG -) - -FetchContent_MakeAvailable(xtl xtensor Eigen3) +FetchContent_MakeAvailable(xtl xtensor) # Pull in our subdirectories add_subdirectory(src/viam) diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index cc7d46dc5..f4e5ac518 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -305,7 +305,6 @@ target_link_libraries(viamsdk PUBLIC Boost::log PUBLIC Boost::log_setup PUBLIC xtensor - PUBLIC Eigen3::Eigen PRIVATE ${VIAMCPPSDK_GRPCXX_REFLECTION_LIBRARIES} PRIVATE ${VIAMCPPSDK_GRPCXX_LIBRARIES} PRIVATE ${VIAMCPPSDK_GRPC_LIBRARIES} diff --git a/src/viam/sdk/common/linear_algebra.hpp b/src/viam/sdk/common/linear_algebra.hpp index 58de611bb..4018539eb 100644 --- a/src/viam/sdk/common/linear_algebra.hpp +++ b/src/viam/sdk/common/linear_algebra.hpp @@ -38,6 +38,13 @@ struct Vector3 { std::array data; }; +inline bool operator==(const Vector3& a, const Vector3& b) { + return a.data == b.data; +} +inline bool operator!=(const Vector3& a, const Vector3& b) { + return !(a == b); +} + namespace proto_convert_details { template <> diff --git a/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp b/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp index 2bb669103..edda19295 100644 --- a/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp +++ b/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp @@ -6,10 +6,10 @@ #include #include -#include #include #include +#include #include namespace viam { @@ -22,9 +22,9 @@ struct ParsedJoint { std::string type_str; ///< raw URDF type attribute std::string parent_link; std::string child_link; - Eigen::Vector3d xyz{0, 0, 0}; - Eigen::Vector3d rpy{0, 0, 0}; - boost::optional axis_opt; + Vector3 xyz{}; + Vector3 rpy{}; + boost::optional axis_opt; }; /// @brief Parse all elements from a URDF, no topology checks. diff --git a/src/viam/sdk/referenceframe/urdf_model_table.cpp b/src/viam/sdk/referenceframe/urdf_model_table.cpp index 1883aeaa5..65c076c20 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.cpp +++ b/src/viam/sdk/referenceframe/urdf_model_table.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -27,14 +28,18 @@ namespace urdf_model_table_internals { namespace { -Eigen::Vector3d parse_triple(const std::string& s) { +Vector3 parse_triple(const std::string& s) { std::istringstream iss(s); double x = 0, y = 0, z = 0; if (!(iss >> x >> y >> z)) { throw Exception(ErrorCondition::k_general, "URDFToModelTable: failed to parse space-delimited triple: '" + s + "'"); } - return Eigen::Vector3d(x, y, z); + return Vector3{x, y, z}; +} + +double magnitude(const Vector3& v) { + return std::sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); } } // namespace @@ -164,16 +169,16 @@ JointRow to_row(const ParsedJoint& parsed) { } if (row.type == JointType::fixed) { - row.axis = Eigen::Vector3d(0, 0, 0); + row.axis = Vector3{0, 0, 0}; } else if (parsed.axis_opt) { - if (parsed.axis_opt->norm() < 1e-12) { + if (magnitude(*parsed.axis_opt) < 1e-12) { throw Exception( ErrorCondition::k_general, "URDFToModelTable: joint '" + parsed.name + "' has zero-magnitude axis"); } row.axis = *parsed.axis_opt; } else { - row.axis = Eigen::Vector3d(1, 0, 0); // URDF default + row.axis = Vector3{1, 0, 0}; // URDF default } return row; } @@ -238,9 +243,9 @@ ModelTable tensor_to_model_table(const xt::xarray& tensor) { " (expected integer in [0, 3])"); } JointRow row; - row.xyz = Eigen::Vector3d(tensor(i, 0), tensor(i, 1), tensor(i, 2)); - row.rpy = Eigen::Vector3d(tensor(i, 3), tensor(i, 4), tensor(i, 5)); - row.axis = Eigen::Vector3d(tensor(i, 6), tensor(i, 7), tensor(i, 8)); + row.xyz = Vector3{tensor(i, 0), tensor(i, 1), tensor(i, 2)}; + row.rpy = Vector3{tensor(i, 3), tensor(i, 4), tensor(i, 5)}; + row.axis = Vector3{tensor(i, 6), tensor(i, 7), tensor(i, 8)}; row.type = static_cast(type_i); table.push_back(std::move(row)); } diff --git a/src/viam/sdk/referenceframe/urdf_model_table.hpp b/src/viam/sdk/referenceframe/urdf_model_table.hpp index 44c43a091..9977d4774 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.hpp +++ b/src/viam/sdk/referenceframe/urdf_model_table.hpp @@ -6,7 +6,6 @@ #include #include -#include #if defined(__has_include) && (__has_include()) #include #else @@ -14,6 +13,7 @@ #endif #include +#include namespace viam { namespace sdk { @@ -24,10 +24,10 @@ enum class JointType { revolute, continuous, prismatic, fixed }; /// @brief One row of the model table: the per-joint URDF fields. /// @note `xyz`/`rpy`/`axis` are taken directly from the URDF. struct JointRow { - std::string name; - Eigen::Vector3d xyz{0, 0, 0}; - Eigen::Vector3d rpy{0, 0, 0}; - Eigen::Vector3d axis{0, 0, 0}; + std::string name; + Vector3 xyz{}; + Vector3 rpy{}; + Vector3 axis{}; JointType type = JointType::fixed; }; diff --git a/src/viam/sdk/tests/test_urdf_model_table.cpp b/src/viam/sdk/tests/test_urdf_model_table.cpp index c1925d950..186ad092d 100644 --- a/src/viam/sdk/tests/test_urdf_model_table.cpp +++ b/src/viam/sdk/tests/test_urdf_model_table.cpp @@ -53,8 +53,8 @@ BOOST_AUTO_TEST_CASE(parse_urdf_missing_origin) { )"; auto parsed = parse_urdf(urdf_from_string(xml)); BOOST_REQUIRE_EQUAL(parsed.size(), 1u); - BOOST_CHECK(parsed[0].xyz.isZero()); - BOOST_CHECK(parsed[0].rpy.isZero()); + BOOST_CHECK(parsed[0].xyz == Vector3{}); + BOOST_CHECK(parsed[0].rpy == Vector3{}); } BOOST_AUTO_TEST_CASE(parse_urdf_missing_axis) { @@ -163,16 +163,16 @@ BOOST_AUTO_TEST_CASE(to_row_each_type) { }; for (const auto& c : cases) { ParsedJoint p = mk("j", "a", "b", c.first); - p.axis_opt = Eigen::Vector3d(0, 0, 1); + p.axis_opt = Vector3{0, 0, 1}; BOOST_CHECK(to_row(p).type == c.second); } } BOOST_AUTO_TEST_CASE(to_row_fixed_zeroes_axis) { ParsedJoint p = mk("j", "a", "b", "fixed"); - p.axis_opt = Eigen::Vector3d(1, 2, 3); + p.axis_opt = Vector3{1, 2, 3}; auto row = to_row(p); - BOOST_CHECK(row.axis.isZero()); + BOOST_CHECK(row.axis == Vector3{}); } BOOST_AUTO_TEST_CASE(to_row_default_axis_for_nonfixed) { @@ -193,7 +193,7 @@ BOOST_AUTO_TEST_CASE(to_row_unsupported_type_throws) { BOOST_AUTO_TEST_CASE(to_row_zero_axis_throws) { ParsedJoint p = mk("j", "a", "b", "revolute"); - p.axis_opt = Eigen::Vector3d(0, 0, 0); + p.axis_opt = Vector3{0, 0, 0}; auto match = [](const Exception& e) { return std::string(e.what()).find("zero-magnitude axis") != std::string::npos; }; @@ -223,7 +223,7 @@ BOOST_AUTO_TEST_CASE(urdf_to_model_table_end_to_end_inline) { BOOST_CHECK_EQUAL(table[1].name, "fix"); BOOST_CHECK(table[1].type == JointType::fixed); - BOOST_CHECK(table[1].axis.isZero()); + BOOST_CHECK(table[1].axis == Vector3{}); } BOOST_AUTO_TEST_CASE(tensor_shape_and_dtype) { @@ -252,7 +252,7 @@ BOOST_AUTO_TEST_CASE(tensor_geometry_columns) { } BOOST_AUTO_TEST_CASE(tensor_type_column) { - const Eigen::Vector3d z{0, 0, 0}; // explicit zero; Eigen{} default-ctor is uninitialized + const Vector3 z{}; std::vector table{ JointRow{"a", z, z, {0, 0, 1}, JointType::revolute}, JointRow{"b", z, z, {0, 0, 1}, JointType::continuous}, @@ -362,7 +362,7 @@ BOOST_AUTO_TEST_CASE(model_table_gp12_has_fixed_tool0) { for (const auto& row : table) { if (row.type == JointType::fixed) { has_fixed_tool = true; - BOOST_CHECK(row.axis.isZero()); + BOOST_CHECK(row.axis == Vector3{}); } } BOOST_CHECK(has_fixed_tool); @@ -417,7 +417,7 @@ BOOST_AUTO_TEST_CASE(model_table_gp12_full_chain) { // Fixed tool joint: nonzero rpy, zero axis (substituted by to_row). BOOST_CHECK_CLOSE(table[6].xyz.x(), 0.100, 1e-9); BOOST_CHECK_CLOSE(table[6].rpy.y(), -1.570796, 1e-3); - BOOST_CHECK(table[6].axis.isZero()); + BOOST_CHECK(table[6].axis == Vector3{}); // Tensor cross-check: same 7 rows, 10 columns, type column at index 9. auto t = model_table_to_tensor(table); @@ -441,7 +441,7 @@ BOOST_AUTO_TEST_CASE(parse_urdf_gp12_full_file) { BOOST_CHECK_SMALL(parsed[0].xyz.x(), 1e-9); BOOST_CHECK_SMALL(parsed[0].xyz.y(), 1e-9); BOOST_CHECK_CLOSE(parsed[0].xyz.z(), 0.450, 1e-9); - BOOST_CHECK(parsed[0].rpy.isZero()); + BOOST_CHECK(parsed[0].rpy == Vector3{}); BOOST_REQUIRE(parsed[0].axis_opt.has_value()); BOOST_CHECK_SMALL(parsed[0].axis_opt->x(), 1e-9); BOOST_CHECK_SMALL(parsed[0].axis_opt->y(), 1e-9); @@ -482,7 +482,7 @@ BOOST_AUTO_TEST_CASE(parse_urdf_gp12_full_file) { BOOST_CHECK_EQUAL(parsed[4].type_str, "revolute"); BOOST_CHECK_EQUAL(parsed[4].parent_link, "link_4_r"); BOOST_CHECK_EQUAL(parsed[4].child_link, "link_5_b"); - BOOST_CHECK(parsed[4].xyz.isZero()); + BOOST_CHECK(parsed[4].xyz == Vector3{}); BOOST_REQUIRE(parsed[4].axis_opt.has_value()); BOOST_CHECK_CLOSE(parsed[4].axis_opt->y(), -1.0, 1e-9); @@ -491,7 +491,7 @@ BOOST_AUTO_TEST_CASE(parse_urdf_gp12_full_file) { BOOST_CHECK_EQUAL(parsed[5].type_str, "revolute"); BOOST_CHECK_EQUAL(parsed[5].parent_link, "link_5_b"); BOOST_CHECK_EQUAL(parsed[5].child_link, "link_6_t"); - BOOST_CHECK(parsed[5].xyz.isZero()); + BOOST_CHECK(parsed[5].xyz == Vector3{}); BOOST_REQUIRE(parsed[5].axis_opt.has_value()); BOOST_CHECK_CLOSE(parsed[5].axis_opt->x(), -1.0, 1e-9); From 2404c5ac01c7f27a5863ab9a10bcdf2c2f630376 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 26 May 2026 11:52:00 -0400 Subject: [PATCH 03/10] should pass CI now --- .../sdk/referenceframe/urdf_model_table.cpp | 32 ++++++++++++------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/viam/sdk/referenceframe/urdf_model_table.cpp b/src/viam/sdk/referenceframe/urdf_model_table.cpp index 65c076c20..4bdec4eca 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.cpp +++ b/src/viam/sdk/referenceframe/urdf_model_table.cpp @@ -30,7 +30,9 @@ namespace { Vector3 parse_triple(const std::string& s) { std::istringstream iss(s); - double x = 0, y = 0, z = 0; + double x = 0; + double y = 0; + double z = 0; if (!(iss >> x >> y >> z)) { throw Exception(ErrorCondition::k_general, "URDFToModelTable: failed to parse space-delimited triple: '" + s + "'"); @@ -47,7 +49,8 @@ double magnitude(const Vector3& v) { std::vector parse_urdf(const KinematicsDataURDF& urdf) { namespace pt = boost::property_tree; pt::ptree tree; - std::string text(reinterpret_cast(urdf.bytes.data()), urdf.bytes.size()); + const std::string text(reinterpret_cast(urdf.bytes.data()), + urdf.bytes.size()); std::istringstream iss(text); try { pt::read_xml(iss, tree); @@ -65,8 +68,9 @@ std::vector parse_urdf(const KinematicsDataURDF& urdf) { std::vector joints; for (const auto& child : robot) { - if (child.first != "joint") + if (child.first != "joint") { continue; + } const pt::ptree& jnode = child.second; ParsedJoint j; @@ -97,7 +101,8 @@ std::vector parse_urdf(const KinematicsDataURDF& urdf) { std::vector walk_urdf_chain(const std::vector& joints) { std::map> by_parent; - std::set all_parents, all_children; + std::set all_parents; + std::set all_children; for (const auto& j : joints) { by_parent[j.parent_link].push_back(&j); all_parents.insert(j.parent_link); @@ -106,13 +111,15 @@ std::vector walk_urdf_chain(const std::vector& joints) std::vector roots; for (const auto& p : all_parents) { - if (all_children.find(p) == all_children.end()) + if (all_children.find(p) == all_children.end()) { roots.push_back(p); + } } if (roots.size() != 1) { std::string list; - for (const auto& r : roots) + for (const auto& r : roots) { list += "'" + r + "' "; + } throw Exception(ErrorCondition::k_general, "URDFToModelTable: expected exactly one root link, found " + std::to_string(roots.size()) + ": " + list); @@ -128,8 +135,9 @@ std::vector walk_urdf_chain(const std::vector& joints) "URDFToModelTable: cycle in URDF chain at link '" + current + "'"); } auto it = by_parent.find(current); - if (it == by_parent.end() || it->second.empty()) + if (it == by_parent.end() || it->second.empty()) { break; + } if (it->second.size() > 1) { throw Exception(ErrorCondition::k_general, "URDFToModelTable: branching topology at link '" + current + "' (" + @@ -153,15 +161,15 @@ JointRow to_row(const ParsedJoint& parsed) { row.xyz = parsed.xyz; row.rpy = parsed.rpy; - if (parsed.type_str == "revolute") + if (parsed.type_str == "revolute") { row.type = JointType::revolute; - else if (parsed.type_str == "continuous") + } else if (parsed.type_str == "continuous") { row.type = JointType::continuous; - else if (parsed.type_str == "prismatic") + } else if (parsed.type_str == "prismatic") { row.type = JointType::prismatic; - else if (parsed.type_str == "fixed") + } else if (parsed.type_str == "fixed") { row.type = JointType::fixed; - else { + } else { throw Exception(ErrorCondition::k_not_supported, "URDFToModelTable: joint '" + parsed.name + "' has unsupported type '" + parsed.type_str + From 96d2b607c072e95a2b789c386c2b47b6a9c2d629 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 26 May 2026 15:37:44 -0400 Subject: [PATCH 04/10] liiiint --- .../mlmodel/example_audio_classification_client.cpp | 6 ++---- src/viam/sdk/module/service.cpp | 4 ++-- src/viam/sdk/referenceframe/urdf_model_table.cpp | 3 +-- src/viam/sdk/spatialmath/geometry.cpp | 9 ++++----- 4 files changed, 9 insertions(+), 13 deletions(-) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 313cbd159..862030b0d 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,11 +404,9 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " - << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " - << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index 8b00c2694..c42e42317 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -241,8 +241,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " - << "`" << dep_name << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " << "`" << dep_name + << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/referenceframe/urdf_model_table.cpp b/src/viam/sdk/referenceframe/urdf_model_table.cpp index 4bdec4eca..139d692bb 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.cpp +++ b/src/viam/sdk/referenceframe/urdf_model_table.cpp @@ -49,8 +49,7 @@ double magnitude(const Vector3& v) { std::vector parse_urdf(const KinematicsDataURDF& urdf) { namespace pt = boost::property_tree; pt::ptree tree; - const std::string text(reinterpret_cast(urdf.bytes.data()), - urdf.bytes.size()); + const std::string text(reinterpret_cast(urdf.bytes.data()), urdf.bytes.size()); std::istringstream iss(text); try { pt::read_xml(iss, tree); diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index 6dbc4b0a0..e4f39baa4 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,11 +57,10 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " - << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z - << "},\norientation: " - << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y - << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y + << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x + << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z + << "},\ntheta: " << v.theta << "}"; return os; } From 35be7285a8135d11dce1db9695d29dcc000615d5 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 26 May 2026 15:48:58 -0400 Subject: [PATCH 05/10] lets try now? --- .../mlmodel/example_audio_classification_client.cpp | 6 ++++-- src/viam/sdk/module/service.cpp | 4 ++-- src/viam/sdk/spatialmath/geometry.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 862030b0d..313cbd159 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,9 +404,11 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " + << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " + << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index ce7cc92c3..4ef1f0019 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -249,8 +249,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " << "`" << dep_name - << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " + << "`" << dep_name << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index e4f39baa4..6dbc4b0a0 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,10 +57,11 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y - << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x - << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z - << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " + << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z + << "},\norientation: " + << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y + << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; return os; } From 71322014b8bab40fede4a973f1c47c8835918910 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 27 May 2026 16:14:53 -0400 Subject: [PATCH 06/10] rename --- .../example_audio_classification_client.cpp | 6 ++---- src/viam/sdk/CMakeLists.txt | 4 ++-- src/viam/sdk/module/service.cpp | 4 ++-- ...el_table.cpp => kinematics_model_table.cpp} | 6 +++--- ...el_table.hpp => kinematics_model_table.hpp} | 8 +++++--- ...pp => kinematics_model_table_internals.hpp} | 8 ++++---- src/viam/sdk/spatialmath/geometry.cpp | 9 ++++----- src/viam/sdk/tests/CMakeLists.txt | 4 ++-- ...ble.cpp => test_kinematics_model_table.cpp} | 18 +++++++++--------- 9 files changed, 33 insertions(+), 34 deletions(-) rename src/viam/sdk/referenceframe/{urdf_model_table.cpp => kinematics_model_table.cpp} (97%) rename src/viam/sdk/referenceframe/{urdf_model_table.hpp => kinematics_model_table.hpp} (86%) rename src/viam/sdk/referenceframe/private/{urdf_model_table_internals.hpp => kinematics_model_table_internals.hpp} (81%) rename src/viam/sdk/tests/{test_urdf_model_table.cpp => test_kinematics_model_table.cpp} (97%) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 313cbd159..862030b0d 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,11 +404,9 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " - << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " - << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index f4e5ac518..1cde78387 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -139,7 +139,7 @@ target_sources(viamsdk module/service.cpp module/private/data_consumer_query.cpp referenceframe/frame.cpp - referenceframe/urdf_model_table.cpp + referenceframe/kinematics_model_table.cpp registry/registry.cpp resource/resource.cpp resource/resource_api.cpp @@ -220,7 +220,7 @@ target_sources(viamsdk ../../viam/sdk/module/service.hpp ../../viam/sdk/module/signal_manager.hpp ../../viam/sdk/referenceframe/frame.hpp - ../../viam/sdk/referenceframe/urdf_model_table.hpp + ../../viam/sdk/referenceframe/kinematics_model_table.hpp ../../viam/sdk/registry/registry.hpp ../../viam/sdk/resource/resource.hpp ../../viam/sdk/resource/resource_api.hpp diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index 4ef1f0019..ce7cc92c3 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -249,8 +249,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " - << "`" << dep_name << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " << "`" << dep_name + << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/referenceframe/urdf_model_table.cpp b/src/viam/sdk/referenceframe/kinematics_model_table.cpp similarity index 97% rename from src/viam/sdk/referenceframe/urdf_model_table.cpp rename to src/viam/sdk/referenceframe/kinematics_model_table.cpp index 139d692bb..bf7067cf8 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.cpp +++ b/src/viam/sdk/referenceframe/kinematics_model_table.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -20,7 +20,7 @@ #endif #include -#include +#include namespace viam { namespace sdk { @@ -192,7 +192,7 @@ JointRow to_row(const ParsedJoint& parsed) { } // namespace urdf_model_table_internals -ModelTable urdf_to_model_table(const KinematicsDataURDF& urdf) { +ModelTable kinematics_to_model_table(const KinematicsDataURDF& urdf) { using namespace urdf_model_table_internals; auto chain = walk_urdf_chain(parse_urdf(urdf)); ModelTable table; diff --git a/src/viam/sdk/referenceframe/urdf_model_table.hpp b/src/viam/sdk/referenceframe/kinematics_model_table.hpp similarity index 86% rename from src/viam/sdk/referenceframe/urdf_model_table.hpp rename to src/viam/sdk/referenceframe/kinematics_model_table.hpp index 9977d4774..4328dd58e 100644 --- a/src/viam/sdk/referenceframe/urdf_model_table.hpp +++ b/src/viam/sdk/referenceframe/kinematics_model_table.hpp @@ -1,4 +1,4 @@ -/// @file referenceframe/urdf_model_table.hpp +/// @file referenceframe/kinematics_model_table.hpp /// @brief Convert a serial URDF chain to a typed per-joint model table /// and from the table to a double (n, 10) tensor. #pragma once @@ -33,10 +33,12 @@ struct JointRow { using ModelTable = std::vector; -/// @brief Parse a URDF and return the per-joint model table in chain order. +/// @brief Parse a kinematics description and return the per-joint model +/// table in chain order. Currently only URDF input is supported; SVA +/// support is planned. /// @throws viam::sdk::Exception on parse error, branching / multi-root / /// disconnected topology, unsupported joint types, or zero axis. -ModelTable urdf_to_model_table(const KinematicsDataURDF& urdf); +ModelTable kinematics_to_model_table(const KinematicsDataURDF& urdf); /// @brief Convert a model table to a double tensor of shape (n, 10). /// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type as diff --git a/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp similarity index 81% rename from src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp rename to src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp index edda19295..b9130f458 100644 --- a/src/viam/sdk/referenceframe/private/urdf_model_table_internals.hpp +++ b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp @@ -1,6 +1,6 @@ -/// @file referenceframe/private/urdf_model_table_internals.hpp -/// @brief Internal helpers for urdf_to_model_table. Exposed only to -/// the testsuite; not a stable public API. +/// @file referenceframe/private/kinematics_model_table_internals.hpp +/// @brief Internal helpers for kinematics_to_model_table (URDF path). +/// Exposed only to the testsuite; not a stable public API. #pragma once #include @@ -10,7 +10,7 @@ #include #include -#include +#include namespace viam { namespace sdk { diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index 6dbc4b0a0..e4f39baa4 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,11 +57,10 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " - << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z - << "},\norientation: " - << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y - << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y + << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x + << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z + << "},\ntheta: " << v.theta << "}"; return os; } diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index f79854f21..1319b1abc 100644 --- a/src/viam/sdk/tests/CMakeLists.txt +++ b/src/viam/sdk/tests/CMakeLists.txt @@ -85,9 +85,9 @@ viamcppsdk_add_boost_test(test_sensor.cpp) viamcppsdk_add_boost_test(test_servo.cpp) viamcppsdk_add_boost_test(test_switch.cpp) viamcppsdk_add_boost_test(test_robot.cpp) -viamcppsdk_add_boost_test(test_urdf_model_table.cpp) +viamcppsdk_add_boost_test(test_kinematics_model_table.cpp) -target_compile_definitions(test_urdf_model_table +target_compile_definitions(test_kinematics_model_table PRIVATE VIAMCPPSDK_TEST_TESTFILES_DIR="${CMAKE_CURRENT_SOURCE_DIR}/testfiles" ) diff --git a/src/viam/sdk/tests/test_urdf_model_table.cpp b/src/viam/sdk/tests/test_kinematics_model_table.cpp similarity index 97% rename from src/viam/sdk/tests/test_urdf_model_table.cpp rename to src/viam/sdk/tests/test_kinematics_model_table.cpp index 186ad092d..aea2f2ffa 100644 --- a/src/viam/sdk/tests/test_urdf_model_table.cpp +++ b/src/viam/sdk/tests/test_kinematics_model_table.cpp @@ -1,4 +1,4 @@ -#define BOOST_TEST_MODULE test module test_urdf_model_table +#define BOOST_TEST_MODULE test module test_kinematics_model_table #include #include @@ -7,8 +7,8 @@ #include #include -#include -#include +#include +#include using namespace viam::sdk; using namespace viam::sdk::urdf_model_table_internals; @@ -200,7 +200,7 @@ BOOST_AUTO_TEST_CASE(to_row_zero_axis_throws) { BOOST_CHECK_EXCEPTION(to_row(p), Exception, match); } -BOOST_AUTO_TEST_CASE(urdf_to_model_table_end_to_end_inline) { +BOOST_AUTO_TEST_CASE(kinematics_to_model_table_end_to_end_inline) { const std::string xml = R"( @@ -213,7 +213,7 @@ BOOST_AUTO_TEST_CASE(urdf_to_model_table_end_to_end_inline) { )"; - auto table = urdf_to_model_table(urdf_from_string(xml)); + auto table = kinematics_to_model_table(urdf_from_string(xml)); BOOST_REQUIRE_EQUAL(table.size(), 2u); BOOST_CHECK_EQUAL(table[0].name, "j1"); @@ -357,7 +357,7 @@ KinematicsDataURDF load_urdf(const std::string& filename) { } // namespace BOOST_AUTO_TEST_CASE(model_table_gp12_has_fixed_tool0) { - auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto table = kinematics_to_model_table(load_urdf("gp12.urdf")); bool has_fixed_tool = false; for (const auto& row : table) { if (row.type == JointType::fixed) { @@ -369,7 +369,7 @@ BOOST_AUTO_TEST_CASE(model_table_gp12_has_fixed_tool0) { } BOOST_AUTO_TEST_CASE(tensor_round_trip_gp12) { - auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto table = kinematics_to_model_table(load_urdf("gp12.urdf")); auto t = model_table_to_tensor(table); BOOST_CHECK_EQUAL(t.shape()[0], table.size()); BOOST_CHECK_EQUAL(t.shape()[1], 10u); @@ -381,7 +381,7 @@ BOOST_AUTO_TEST_CASE(tensor_round_trip_gp12) { } BOOST_AUTO_TEST_CASE(model_table_gp12_full_chain) { - auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto table = kinematics_to_model_table(load_urdf("gp12.urdf")); // 6 revolute joints + 1 fixed tool joint. BOOST_REQUIRE_EQUAL(table.size(), 7u); @@ -510,7 +510,7 @@ BOOST_AUTO_TEST_CASE(parse_urdf_gp12_full_file) { } BOOST_AUTO_TEST_CASE(tensor_to_model_table_gp12_round_trip) { - auto table = urdf_to_model_table(load_urdf("gp12.urdf")); + auto table = kinematics_to_model_table(load_urdf("gp12.urdf")); auto t = model_table_to_tensor(table); auto rebuilt = tensor_to_model_table(t); BOOST_REQUIRE_EQUAL(rebuilt.size(), table.size()); From 41207a10af7db9cccf1093f8cbfcc314d9046c85 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 27 May 2026 16:19:07 -0400 Subject: [PATCH 07/10] linter --- .../mlmodel/example_audio_classification_client.cpp | 6 ++++-- src/viam/sdk/module/service.cpp | 4 ++-- src/viam/sdk/spatialmath/geometry.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 862030b0d..313cbd159 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,9 +404,11 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " + << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " + << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index ce7cc92c3..4ef1f0019 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -249,8 +249,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " << "`" << dep_name - << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " + << "`" << dep_name << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index e4f39baa4..6dbc4b0a0 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,10 +57,11 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y - << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x - << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z - << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " + << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z + << "},\norientation: " + << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y + << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; return os; } From dbe6b9b675ca50398f9a32c3c68d523554ab86fd Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 29 May 2026 16:37:42 -0400 Subject: [PATCH 08/10] requested changes --- conanfile.py | 2 - .../example_audio_classification_client.cpp | 6 +-- src/viam/sdk/common/linear_algebra.hpp | 1 + src/viam/sdk/module/service.cpp | 4 +- .../referenceframe/kinematics_model_table.cpp | 38 ++++++++++----- .../referenceframe/kinematics_model_table.hpp | 19 +++++--- .../kinematics_model_table_internals.hpp | 4 +- src/viam/sdk/spatialmath/geometry.cpp | 9 ++-- .../sdk/tests/test_kinematics_model_table.cpp | 48 +++++++++---------- 9 files changed, 74 insertions(+), 57 deletions(-) diff --git a/conanfile.py b/conanfile.py index 2db6ca616..06ff387ce 100644 --- a/conanfile.py +++ b/conanfile.py @@ -91,7 +91,6 @@ def requirements(self): self.requires(self._grpc_requires()) self.requires('protobuf/[>=3.17.1 <6.30.0]') self.requires(self._xtensor_requires(), transitive_headers=True) - self.requires('eigen/[>=3.3.0]', transitive_headers=True) if self.options.opentelemetry_tracing: # Oldest maintained conan package and first version with proper CMake support @@ -190,7 +189,6 @@ def package_info(self): "boost::log", "grpc::grpc++_reflection", "xtensor::xtensor", - "eigen::eigen", "viamapi", ]) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 313cbd159..862030b0d 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,11 +404,9 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " - << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " - << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/common/linear_algebra.hpp b/src/viam/sdk/common/linear_algebra.hpp index 4018539eb..5c4f3fc4f 100644 --- a/src/viam/sdk/common/linear_algebra.hpp +++ b/src/viam/sdk/common/linear_algebra.hpp @@ -41,6 +41,7 @@ struct Vector3 { inline bool operator==(const Vector3& a, const Vector3& b) { return a.data == b.data; } + inline bool operator!=(const Vector3& a, const Vector3& b) { return !(a == b); } diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index 4ef1f0019..ce7cc92c3 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -249,8 +249,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " - << "`" << dep_name << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " << "`" << dep_name + << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/referenceframe/kinematics_model_table.cpp b/src/viam/sdk/referenceframe/kinematics_model_table.cpp index bf7067cf8..1d2181dc6 100644 --- a/src/viam/sdk/referenceframe/kinematics_model_table.cpp +++ b/src/viam/sdk/referenceframe/kinematics_model_table.cpp @@ -24,7 +24,7 @@ namespace viam { namespace sdk { -namespace urdf_model_table_internals { +namespace impl { namespace { @@ -161,13 +161,13 @@ JointRow to_row(const ParsedJoint& parsed) { row.rpy = parsed.rpy; if (parsed.type_str == "revolute") { - row.type = JointType::revolute; + row.type = JointType::k_revolute; } else if (parsed.type_str == "continuous") { - row.type = JointType::continuous; + row.type = JointType::k_continuous; } else if (parsed.type_str == "prismatic") { - row.type = JointType::prismatic; + row.type = JointType::k_prismatic; } else if (parsed.type_str == "fixed") { - row.type = JointType::fixed; + row.type = JointType::k_fixed; } else { throw Exception(ErrorCondition::k_not_supported, "URDFToModelTable: joint '" + parsed.name + "' has unsupported type '" + @@ -175,7 +175,7 @@ JointRow to_row(const ParsedJoint& parsed) { "' (supported: revolute, continuous, prismatic, fixed)"); } - if (row.type == JointType::fixed) { + if (row.type == JointType::k_fixed) { row.axis = Vector3{0, 0, 0}; } else if (parsed.axis_opt) { if (magnitude(*parsed.axis_opt) < 1e-12) { @@ -190,10 +190,10 @@ JointRow to_row(const ParsedJoint& parsed) { return row; } -} // namespace urdf_model_table_internals +} // namespace impl ModelTable kinematics_to_model_table(const KinematicsDataURDF& urdf) { - using namespace urdf_model_table_internals; + using namespace impl; auto chain = walk_urdf_chain(parse_urdf(urdf)); ModelTable table; table.reserve(chain.size()); @@ -243,17 +243,31 @@ ModelTable tensor_to_model_table(const xt::xarray& tensor) { for (std::size_t i = 0; i < tensor.shape()[0]; ++i) { const double type_v = tensor(i, 9); const int type_i = static_cast(type_v); - if (static_cast(type_i) != type_v || type_i < 0 || type_i > 3) { + if (static_cast(type_i) != type_v) { throw Exception(ErrorCondition::k_general, "tensor_to_model_table: row " + std::to_string(i) + - " has invalid joint type value " + std::to_string(type_v) + - " (expected integer in [0, 3])"); + " joint type value " + std::to_string(type_v) + + " is not an integer"); + } + JointType joint_type; + switch (static_cast(type_i)) { + case JointType::k_revolute: + case JointType::k_continuous: + case JointType::k_prismatic: + case JointType::k_fixed: + joint_type = static_cast(type_i); + break; + default: + throw Exception(ErrorCondition::k_general, + "tensor_to_model_table: row " + std::to_string(i) + + " joint type value " + std::to_string(type_i) + + " does not match any JointType"); } JointRow row; row.xyz = Vector3{tensor(i, 0), tensor(i, 1), tensor(i, 2)}; row.rpy = Vector3{tensor(i, 3), tensor(i, 4), tensor(i, 5)}; row.axis = Vector3{tensor(i, 6), tensor(i, 7), tensor(i, 8)}; - row.type = static_cast(type_i); + row.type = joint_type; table.push_back(std::move(row)); } return table; diff --git a/src/viam/sdk/referenceframe/kinematics_model_table.hpp b/src/viam/sdk/referenceframe/kinematics_model_table.hpp index 4328dd58e..66c36f1b0 100644 --- a/src/viam/sdk/referenceframe/kinematics_model_table.hpp +++ b/src/viam/sdk/referenceframe/kinematics_model_table.hpp @@ -19,7 +19,14 @@ namespace viam { namespace sdk { /// @brief URDF joint type, restricted to arm-relevant joints. -enum class JointType { revolute, continuous, prismatic, fixed }; +/// @note Underlying values are stable and form the wire encoding for +/// column 9 of the tensor produced by `model_table_to_tensor`. +enum class JointType { + k_revolute = 0, + k_continuous = 1, + k_prismatic = 2, + k_fixed = 3, +}; /// @brief One row of the model table: the per-joint URDF fields. /// @note `xyz`/`rpy`/`axis` are taken directly from the URDF. @@ -28,7 +35,7 @@ struct JointRow { Vector3 xyz{}; Vector3 rpy{}; Vector3 axis{}; - JointType type = JointType::fixed; + JointType type = JointType::k_fixed; }; using ModelTable = std::vector; @@ -41,8 +48,8 @@ using ModelTable = std::vector; ModelTable kinematics_to_model_table(const KinematicsDataURDF& urdf); /// @brief Convert a model table to a double tensor of shape (n, 10). -/// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type as -/// underlying enum value (0=revolute, 1=continuous, 2=prismatic, 3=fixed). +/// Columns: 0..2 xyz, 3..5 rpy, 6..8 axis, 9 joint type as the underlying +/// value of `JointType` (see enum declaration for the encoding). /// @throws viam::sdk::Exception on empty input. xt::xarray model_table_to_tensor(const ModelTable& table); @@ -50,8 +57,8 @@ xt::xarray model_table_to_tensor(const ModelTable& table); /// tensor into a model table. Reconstructed rows have empty `name` /// (names are not carried in the tensor). /// @throws viam::sdk::Exception on non-2D input, wrong column count, -/// empty input, or invalid joint-type encoding (col 9 must be an -/// integer in [0, 3]). +/// empty input, or invalid joint-type encoding (col 9 must be an integer +/// matching one of the `JointType` values). ModelTable tensor_to_model_table(const xt::xarray& tensor); } // namespace sdk diff --git a/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp index b9130f458..efed732f0 100644 --- a/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp +++ b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp @@ -14,7 +14,7 @@ namespace viam { namespace sdk { -namespace urdf_model_table_internals { +namespace impl { /// @brief Per-joint parse result, pre-validation. struct ParsedJoint { @@ -38,6 +38,6 @@ std::vector walk_urdf_chain(const std::vector& joints) /// @throws viam::sdk::Exception on unsupported type or zero non-fixed axis. JointRow to_row(const ParsedJoint& parsed); -} // namespace urdf_model_table_internals +} // namespace impl } // namespace sdk } // namespace viam diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index 6dbc4b0a0..e4f39baa4 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,11 +57,10 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " - << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z - << "},\norientation: " - << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y - << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y + << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x + << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z + << "},\ntheta: " << v.theta << "}"; return os; } diff --git a/src/viam/sdk/tests/test_kinematics_model_table.cpp b/src/viam/sdk/tests/test_kinematics_model_table.cpp index aea2f2ffa..c56d8cab2 100644 --- a/src/viam/sdk/tests/test_kinematics_model_table.cpp +++ b/src/viam/sdk/tests/test_kinematics_model_table.cpp @@ -11,7 +11,7 @@ #include using namespace viam::sdk; -using namespace viam::sdk::urdf_model_table_internals; +using namespace viam::sdk::impl; namespace { KinematicsDataURDF urdf_from_string(const std::string& xml) { @@ -156,10 +156,10 @@ BOOST_AUTO_TEST_CASE(walk_urdf_chain_cycle_throws) { BOOST_AUTO_TEST_CASE(to_row_each_type) { const std::vector> cases{ - {"revolute", JointType::revolute}, - {"continuous", JointType::continuous}, - {"prismatic", JointType::prismatic}, - {"fixed", JointType::fixed}, + {"revolute", JointType::k_revolute}, + {"continuous", JointType::k_continuous}, + {"prismatic", JointType::k_prismatic}, + {"fixed", JointType::k_fixed}, }; for (const auto& c : cases) { ParsedJoint p = mk("j", "a", "b", c.first); @@ -217,19 +217,19 @@ BOOST_AUTO_TEST_CASE(kinematics_to_model_table_end_to_end_inline) { BOOST_REQUIRE_EQUAL(table.size(), 2u); BOOST_CHECK_EQUAL(table[0].name, "j1"); - BOOST_CHECK(table[0].type == JointType::revolute); + BOOST_CHECK(table[0].type == JointType::k_revolute); BOOST_CHECK_CLOSE(table[0].xyz.z(), 0.5, 1e-9); BOOST_CHECK_CLOSE(table[0].axis.z(), 1.0, 1e-9); BOOST_CHECK_EQUAL(table[1].name, "fix"); - BOOST_CHECK(table[1].type == JointType::fixed); + BOOST_CHECK(table[1].type == JointType::k_fixed); BOOST_CHECK(table[1].axis == Vector3{}); } BOOST_AUTO_TEST_CASE(tensor_shape_and_dtype) { std::vector table{ - JointRow{"a", {1, 2, 3}, {0.1, 0.2, 0.3}, {0, 0, 1}, JointType::revolute}, - JointRow{"b", {4, 5, 6}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + JointRow{"a", {1, 2, 3}, {0.1, 0.2, 0.3}, {0, 0, 1}, JointType::k_revolute}, + JointRow{"b", {4, 5, 6}, {0, 0, 0}, {0, 0, 0}, JointType::k_fixed}, }; auto t = model_table_to_tensor(table); BOOST_CHECK_EQUAL(t.dimension(), 2u); @@ -240,7 +240,7 @@ BOOST_AUTO_TEST_CASE(tensor_shape_and_dtype) { BOOST_AUTO_TEST_CASE(tensor_geometry_columns) { std::vector table{ - JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::revolute}, + JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::k_revolute}, }; auto t = model_table_to_tensor(table); BOOST_CHECK_CLOSE(t(0, 0), 1.5, 1e-9); @@ -254,10 +254,10 @@ BOOST_AUTO_TEST_CASE(tensor_geometry_columns) { BOOST_AUTO_TEST_CASE(tensor_type_column) { const Vector3 z{}; std::vector table{ - JointRow{"a", z, z, {0, 0, 1}, JointType::revolute}, - JointRow{"b", z, z, {0, 0, 1}, JointType::continuous}, - JointRow{"c", z, z, {1, 0, 0}, JointType::prismatic}, - JointRow{"d", z, z, {0, 0, 0}, JointType::fixed}, + JointRow{"a", z, z, {0, 0, 1}, JointType::k_revolute}, + JointRow{"b", z, z, {0, 0, 1}, JointType::k_continuous}, + JointRow{"c", z, z, {1, 0, 0}, JointType::k_prismatic}, + JointRow{"d", z, z, {0, 0, 0}, JointType::k_fixed}, }; auto t = model_table_to_tensor(table); BOOST_CHECK_EQUAL(t(0, 9), 0.0); // revolute @@ -268,7 +268,7 @@ BOOST_AUTO_TEST_CASE(tensor_type_column) { BOOST_AUTO_TEST_CASE(tensor_fixed_joint_axis_zero) { std::vector table{ - JointRow{"f", {0.1, 0.2, 0.3}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + JointRow{"f", {0.1, 0.2, 0.3}, {0, 0, 0}, {0, 0, 0}, JointType::k_fixed}, }; auto t = model_table_to_tensor(table); BOOST_CHECK_EQUAL(t(0, 6), 0.0); @@ -286,9 +286,9 @@ BOOST_AUTO_TEST_CASE(model_table_to_tensor_empty_throws) { BOOST_AUTO_TEST_CASE(tensor_to_model_table_round_trip) { std::vector table{ - JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::revolute}, - JointRow{"b", {4, 5, 6}, {0, 0, 0}, {1, 0, 0}, JointType::prismatic}, - JointRow{"c", {7, 8, 9}, {0, 0, 0}, {0, 0, 0}, JointType::fixed}, + JointRow{"a", {1.5, 2.5, 3.5}, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, JointType::k_revolute}, + JointRow{"b", {4, 5, 6}, {0, 0, 0}, {1, 0, 0}, JointType::k_prismatic}, + JointRow{"c", {7, 8, 9}, {0, 0, 0}, {0, 0, 0}, JointType::k_fixed}, }; auto t = model_table_to_tensor(table); auto rebuilt = tensor_to_model_table(t); @@ -331,16 +331,16 @@ BOOST_AUTO_TEST_CASE(tensor_to_model_table_fractional_type_throws) { xt::xarray t = xt::zeros({std::size_t{1}, std::size_t{10}}); t(0, 9) = 0.5; // non-integer joint type auto match = [](const Exception& e) { - return std::string(e.what()).find("invalid joint type value") != std::string::npos; + return std::string(e.what()).find("is not an integer") != std::string::npos; }; BOOST_CHECK_EXCEPTION(tensor_to_model_table(t), Exception, match); } BOOST_AUTO_TEST_CASE(tensor_to_model_table_out_of_range_type_throws) { xt::xarray t = xt::zeros({std::size_t{1}, std::size_t{10}}); - t(0, 9) = 7.0; // outside [0, 3] + t(0, 9) = 7.0; // does not match any JointType value auto match = [](const Exception& e) { - return std::string(e.what()).find("invalid joint type value") != std::string::npos; + return std::string(e.what()).find("does not match any JointType") != std::string::npos; }; BOOST_CHECK_EXCEPTION(tensor_to_model_table(t), Exception, match); } @@ -360,7 +360,7 @@ BOOST_AUTO_TEST_CASE(model_table_gp12_has_fixed_tool0) { auto table = kinematics_to_model_table(load_urdf("gp12.urdf")); bool has_fixed_tool = false; for (const auto& row : table) { - if (row.type == JointType::fixed) { + if (row.type == JointType::k_fixed) { has_fixed_tool = true; BOOST_CHECK(row.axis == Vector3{}); } @@ -397,9 +397,9 @@ BOOST_AUTO_TEST_CASE(model_table_gp12_full_chain) { // Types: 6 revolute then 1 fixed. for (std::size_t i = 0; i < 6; ++i) { - BOOST_CHECK(table[i].type == JointType::revolute); + BOOST_CHECK(table[i].type == JointType::k_revolute); } - BOOST_CHECK(table[6].type == JointType::fixed); + BOOST_CHECK(table[6].type == JointType::k_fixed); // Joint 1: shoulder, vertical axis at z=0.450. BOOST_CHECK_SMALL(table[0].xyz.x(), 1e-9); From dcb17e1e8633802f65cb833e58349389a5077b9e Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 29 May 2026 16:45:29 -0400 Subject: [PATCH 09/10] pass ci --- .../mlmodel/example_audio_classification_client.cpp | 6 ++++-- src/viam/sdk/module/service.cpp | 4 ++-- src/viam/sdk/spatialmath/geometry.cpp | 9 +++++---- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/viam/examples/mlmodel/example_audio_classification_client.cpp b/src/viam/examples/mlmodel/example_audio_classification_client.cpp index 862030b0d..313cbd159 100644 --- a/src/viam/examples/mlmodel/example_audio_classification_client.cpp +++ b/src/viam/examples/mlmodel/example_audio_classification_client.cpp @@ -404,9 +404,11 @@ int main(int argc, char* argv[]) try { return EXIT_SUCCESS; } } catch (const std::exception& ex) { - std::cerr << argv[0] << ": " << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; + std::cerr << argv[0] << ": " + << "Failed: a std::exception was thrown: `" << ex.what() << "``\n"; return EXIT_FAILURE; } catch (...) { - std::cerr << argv[0] << ": " << "Failed: an unknown exception was thrown\n"; + std::cerr << argv[0] << ": " + << "Failed: an unknown exception was thrown\n"; return EXIT_FAILURE; } diff --git a/src/viam/sdk/module/service.cpp b/src/viam/sdk/module/service.cpp index ce7cc92c3..4ef1f0019 100644 --- a/src/viam/sdk/module/service.cpp +++ b/src/viam/sdk/module/service.cpp @@ -249,8 +249,8 @@ Dependencies ModuleService::get_dependencies_( const std::shared_ptr dep_resource = get_parent_resource_(dep_name); if (!dep_resource) { std::ostringstream buffer; - buffer << resource_name << ": Dependency " << "`" << dep_name - << "` was not found during (re)configuration"; + buffer << resource_name << ": Dependency " + << "`" << dep_name << "` was not found during (re)configuration"; throw Exception(ErrorCondition::k_resource_not_found, buffer.str()); } deps.emplace(dep_name, dep_resource); diff --git a/src/viam/sdk/spatialmath/geometry.cpp b/src/viam/sdk/spatialmath/geometry.cpp index e4f39baa4..6dbc4b0a0 100644 --- a/src/viam/sdk/spatialmath/geometry.cpp +++ b/src/viam/sdk/spatialmath/geometry.cpp @@ -57,10 +57,11 @@ const std::string& GeometryConfig::get_label() const { } std::ostream& operator<<(std::ostream& os, const pose& v) { - os << "{ coordinates: " << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y - << ", z: " << v.coordinates.z << "},\norientation: " << "{ o_x: " << v.orientation.o_x - << ", o_y: " << v.orientation.o_y << ", o_z: " << v.orientation.o_z - << "},\ntheta: " << v.theta << "}"; + os << "{ coordinates: " + << "{ x: " << v.coordinates.x << ", y: " << v.coordinates.y << ", z: " << v.coordinates.z + << "},\norientation: " + << "{ o_x: " << v.orientation.o_x << ", o_y: " << v.orientation.o_y + << ", o_z: " << v.orientation.o_z << "},\ntheta: " << v.theta << "}"; return os; } From 0aea0e1065d08e710ef479f72bfa85804fd11bc0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 29 May 2026 16:48:22 -0400 Subject: [PATCH 10/10] no number --- .../sdk/referenceframe/kinematics_model_table.cpp | 12 +++--------- src/viam/sdk/tests/test_kinematics_model_table.cpp | 2 +- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/viam/sdk/referenceframe/kinematics_model_table.cpp b/src/viam/sdk/referenceframe/kinematics_model_table.cpp index 1d2181dc6..2141dcbfd 100644 --- a/src/viam/sdk/referenceframe/kinematics_model_table.cpp +++ b/src/viam/sdk/referenceframe/kinematics_model_table.cpp @@ -1,6 +1,5 @@ #include -#include #include #include #include @@ -40,10 +39,6 @@ Vector3 parse_triple(const std::string& s) { return Vector3{x, y, z}; } -double magnitude(const Vector3& v) { - return std::sqrt(v.x() * v.x() + v.y() * v.y() + v.z() * v.z()); -} - } // namespace std::vector parse_urdf(const KinematicsDataURDF& urdf) { @@ -178,10 +173,9 @@ JointRow to_row(const ParsedJoint& parsed) { if (row.type == JointType::k_fixed) { row.axis = Vector3{0, 0, 0}; } else if (parsed.axis_opt) { - if (magnitude(*parsed.axis_opt) < 1e-12) { - throw Exception( - ErrorCondition::k_general, - "URDFToModelTable: joint '" + parsed.name + "' has zero-magnitude axis"); + if (*parsed.axis_opt == Vector3{}) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: joint '" + parsed.name + "' has zero axis"); } row.axis = *parsed.axis_opt; } else { diff --git a/src/viam/sdk/tests/test_kinematics_model_table.cpp b/src/viam/sdk/tests/test_kinematics_model_table.cpp index c56d8cab2..31481080b 100644 --- a/src/viam/sdk/tests/test_kinematics_model_table.cpp +++ b/src/viam/sdk/tests/test_kinematics_model_table.cpp @@ -195,7 +195,7 @@ BOOST_AUTO_TEST_CASE(to_row_zero_axis_throws) { ParsedJoint p = mk("j", "a", "b", "revolute"); p.axis_opt = Vector3{0, 0, 0}; auto match = [](const Exception& e) { - return std::string(e.what()).find("zero-magnitude axis") != std::string::npos; + return std::string(e.what()).find("zero axis") != std::string::npos; }; BOOST_CHECK_EXCEPTION(to_row(p), Exception, match); }