diff --git a/src/viam/sdk/CMakeLists.txt b/src/viam/sdk/CMakeLists.txt index 52779c5ba..1cde78387 100644 --- a/src/viam/sdk/CMakeLists.txt +++ b/src/viam/sdk/CMakeLists.txt @@ -139,6 +139,7 @@ target_sources(viamsdk module/service.cpp module/private/data_consumer_query.cpp referenceframe/frame.cpp + referenceframe/kinematics_model_table.cpp registry/registry.cpp resource/resource.cpp resource/resource_api.cpp @@ -219,6 +220,7 @@ target_sources(viamsdk ../../viam/sdk/module/service.hpp ../../viam/sdk/module/signal_manager.hpp ../../viam/sdk/referenceframe/frame.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/common/linear_algebra.hpp b/src/viam/sdk/common/linear_algebra.hpp index 58de611bb..5c4f3fc4f 100644 --- a/src/viam/sdk/common/linear_algebra.hpp +++ b/src/viam/sdk/common/linear_algebra.hpp @@ -38,6 +38,14 @@ 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/kinematics_model_table.cpp b/src/viam/sdk/referenceframe/kinematics_model_table.cpp new file mode 100644 index 000000000..2141dcbfd --- /dev/null +++ b/src/viam/sdk/referenceframe/kinematics_model_table.cpp @@ -0,0 +1,271 @@ +#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 impl { + +namespace { + +Vector3 parse_triple(const std::string& s) { + std::istringstream iss(s); + 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 + "'"); + } + return Vector3{x, y, z}; +} + +} // namespace + +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()); + 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; + std::set 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::k_revolute; + } else if (parsed.type_str == "continuous") { + row.type = JointType::k_continuous; + } else if (parsed.type_str == "prismatic") { + row.type = JointType::k_prismatic; + } else if (parsed.type_str == "fixed") { + row.type = JointType::k_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::k_fixed) { + row.axis = Vector3{0, 0, 0}; + } else if (parsed.axis_opt) { + if (*parsed.axis_opt == Vector3{}) { + throw Exception(ErrorCondition::k_general, + "URDFToModelTable: joint '" + parsed.name + "' has zero axis"); + } + row.axis = *parsed.axis_opt; + } else { + row.axis = Vector3{1, 0, 0}; // URDF default + } + return row; +} + +} // namespace impl + +ModelTable kinematics_to_model_table(const KinematicsDataURDF& urdf) { + using namespace impl; + 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) { + throw Exception(ErrorCondition::k_general, + "tensor_to_model_table: row " + std::to_string(i) + + " 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 = joint_type; + table.push_back(std::move(row)); + } + return table; +} + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/kinematics_model_table.hpp b/src/viam/sdk/referenceframe/kinematics_model_table.hpp new file mode 100644 index 000000000..66c36f1b0 --- /dev/null +++ b/src/viam/sdk/referenceframe/kinematics_model_table.hpp @@ -0,0 +1,65 @@ +/// @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 + +#include +#include + +#if defined(__has_include) && (__has_include()) +#include +#else +#include +#endif + +#include +#include + +namespace viam { +namespace sdk { + +/// @brief URDF joint type, restricted to arm-relevant joints. +/// @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. +struct JointRow { + std::string name; + Vector3 xyz{}; + Vector3 rpy{}; + Vector3 axis{}; + JointType type = JointType::k_fixed; +}; + +using ModelTable = std::vector; + +/// @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 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 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); + +/// @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 +/// matching one of the `JointType` values). +ModelTable tensor_to_model_table(const xt::xarray& tensor); + +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp new file mode 100644 index 000000000..efed732f0 --- /dev/null +++ b/src/viam/sdk/referenceframe/private/kinematics_model_table_internals.hpp @@ -0,0 +1,43 @@ +/// @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 +#include + +#include + +#include +#include +#include + +namespace viam { +namespace sdk { +namespace impl { + +/// @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; + Vector3 xyz{}; + Vector3 rpy{}; + 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 impl +} // namespace sdk +} // namespace viam diff --git a/src/viam/sdk/tests/CMakeLists.txt b/src/viam/sdk/tests/CMakeLists.txt index fd0f27006..1319b1abc 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_kinematics_model_table.cpp) + +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_kinematics_model_table.cpp b/src/viam/sdk/tests/test_kinematics_model_table.cpp new file mode 100644 index 000000000..31481080b --- /dev/null +++ b/src/viam/sdk/tests/test_kinematics_model_table.cpp @@ -0,0 +1,523 @@ +#define BOOST_TEST_MODULE test module test_kinematics_model_table + +#include +#include +#include + +#include + +#include +#include +#include + +using namespace viam::sdk; +using namespace viam::sdk::impl; + +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 == Vector3{}); + BOOST_CHECK(parsed[0].rpy == Vector3{}); +} + +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::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); + 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 = Vector3{1, 2, 3}; + auto row = to_row(p); + BOOST_CHECK(row.axis == Vector3{}); +} + +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 = Vector3{0, 0, 0}; + auto match = [](const Exception& e) { + return std::string(e.what()).find("zero axis") != std::string::npos; + }; + BOOST_CHECK_EXCEPTION(to_row(p), Exception, match); +} + +BOOST_AUTO_TEST_CASE(kinematics_to_model_table_end_to_end_inline) { + const std::string xml = R"( + + + + + + + + + + +)"; + auto table = kinematics_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::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::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::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); + 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::k_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 Vector3 z{}; + std::vector table{ + 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 + 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::k_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::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); + + 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("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; // does not match any JointType value + auto match = [](const Exception& e) { + return std::string(e.what()).find("does not match any JointType") != 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 = kinematics_to_model_table(load_urdf("gp12.urdf")); + bool has_fixed_tool = false; + for (const auto& row : table) { + if (row.type == JointType::k_fixed) { + has_fixed_tool = true; + BOOST_CHECK(row.axis == Vector3{}); + } + } + BOOST_CHECK(has_fixed_tool); +} + +BOOST_AUTO_TEST_CASE(tensor_round_trip_gp12) { + 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); + 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 = kinematics_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::k_revolute); + } + 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); + 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 == Vector3{}); + + // 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 == 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); + 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 == Vector3{}); + 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 == Vector3{}); + 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 = 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()); + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +