diff --git a/CMakeLists.txt b/CMakeLists.txt index bd4a139..136f668 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -441,6 +441,31 @@ target_link_libraries(streamed_conversion_test add_test(NAME streamed_conversion_test COMMAND streamed_conversion_test) +add_executable(sva_joint_limits_test EXCLUDE_FROM_ALL) + +target_sources(sva_joint_limits_test + PRIVATE + src/viam/module/test/sva_joint_limits_test.cpp + src/viam/module/utils.cpp +) + +target_link_libraries(sva_joint_limits_test + PRIVATE + viam-yaskawa + Boost::unit_test_framework + viam-cpp-sdk::viamsdk + jsoncpp_lib +) + +# Inject the absolute path to the shipped `kinematics/` data so the test can check every model +# document regardless of the cwd it is invoked from. +target_compile_definitions(sva_joint_limits_test + PRIVATE + VIAM_YASKAWA_TEST_KINEMATICS_DIR="${CMAKE_CURRENT_SOURCE_DIR}/src/kinematics" +) + +add_test(NAME sva_joint_limits_test COMMAND sva_joint_limits_test) + # Common sources and include dirs for integration tests that use the fake server set(INTEGRATION_TEST_SOURCES src/viam/lib/test/fake_server.cpp diff --git a/Makefile b/Makefile index 308f872..43dd14b 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,7 @@ install: build ninja -C build install test: configure - cmake --build build --target broadcast_log_parser_test realtime_trajectory_logger_test move_limit_test fault_injection_test controller_integration_test motoplus_flasher_test move_stream_test streamed_conversion_test + cmake --build build --target broadcast_log_parser_test realtime_trajectory_logger_test move_limit_test fault_injection_test controller_integration_test motoplus_flasher_test move_stream_test streamed_conversion_test sva_joint_limits_test ctest --test-dir build --output-on-failure module.tar.gz: format-check install diff --git a/src/viam/module/test/sva_joint_limits_test.cpp b/src/viam/module/test/sva_joint_limits_test.cpp new file mode 100644 index 0000000..b03aa2c --- /dev/null +++ b/src/viam/module/test/sva_joint_limits_test.cpp @@ -0,0 +1,208 @@ +#define BOOST_TEST_MODULE SvaJointLimitsTest +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "../utils.hpp" + +namespace { + +// A two-joint SVA document, trimmed to the fields the patch touches. Keeping it small and local +// means the assertions below are about the patch and not about whichever shipped model we picked. +constexpr char k_sva_two_joints[] = R"({ + "name": "test_arm", + "kinematic_param_type": "SVA", + "links": [ + {"id": "base_link", "parent": "world"}, + {"id": "link_1", "parent": "joint_1"}, + {"id": "link_2", "parent": "joint_2"} + ], + "joints": [ + {"id": "joint_1", "type": "revolute", "parent": "base_link", + "axis": {"x": 0, "y": 0, "z": 1}, "min": -170.0, "max": 170.0}, + {"id": "joint_2", "type": "revolute", "parent": "link_1", + "axis": {"x": 0, "y": 0, "z": 1}, "min": -105.0, "max": 155.0} + ] +})"; + +Json::Value parse(const std::string& text) { + Json::Value root; + std::istringstream in{text}; + const Json::CharReaderBuilder reader_builder; + std::string errs; + BOOST_REQUIRE_MESSAGE(Json::parseFromStream(reader_builder, in, &root, &errs), errs); + return root; +} + +Eigen::VectorXd vec(std::vector values) { + return Eigen::VectorXd::Map(values.data(), static_cast(values.size())).eval(); +} + +} // namespace + +BOOST_AUTO_TEST_SUITE(sva_joint_limits_tests) + +BOOST_AUTO_TEST_CASE(test_limits_are_written_in_degrees_per_joint) { + // Distinct per joint, so transposing the two joints or broadcasting one value fails here. + const auto velocity = vec({degrees_to_radians(110.0), degrees_to_radians(120.0)}); + const auto acceleration = vec({degrees_to_radians(210.0), degrees_to_radians(220.0)}); + + const Json::Value patched = parse(sva_with_joint_limits(k_sva_two_joints, velocity, acceleration)); + + BOOST_REQUIRE_EQUAL(patched["joints"].size(), 2U); + BOOST_CHECK_CLOSE(patched["joints"][0]["max_velocity"].asDouble(), 110.0, 1e-9); + BOOST_CHECK_CLOSE(patched["joints"][0]["max_acceleration"].asDouble(), 210.0, 1e-9); + BOOST_CHECK_CLOSE(patched["joints"][1]["max_velocity"].asDouble(), 120.0, 1e-9); + BOOST_CHECK_CLOSE(patched["joints"][1]["max_acceleration"].asDouble(), 220.0, 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_patch_leaves_the_rest_of_the_document_alone) { + const auto limits = vec({1.0, 1.0}); + const Json::Value patched = parse(sva_with_joint_limits(k_sva_two_joints, limits, limits)); + const Json::Value original = parse(k_sva_two_joints); + + BOOST_CHECK_EQUAL(patched["name"].asString(), "test_arm"); + BOOST_CHECK_EQUAL(patched["kinematic_param_type"].asString(), "SVA"); + BOOST_CHECK(patched["links"] == original["links"]); + + // Position bounds, ids, parents, and axes have to survive, since the motion service still + // needs them and we only meant to add two fields. + for (Json::ArrayIndex i = 0; i < patched["joints"].size(); ++i) { + for (const char* field : {"id", "type", "parent", "axis", "min", "max"}) { + BOOST_CHECK(patched["joints"][i][field] == original["joints"][i][field]); + } + } +} + +BOOST_AUTO_TEST_CASE(test_joint_count_mismatch_throws) { + // Two joints in the document, six limits configured. Patching the first two would publish a + // document that silently disagrees with the config, so this has to fail. + const auto six = Eigen::VectorXd::Constant(k_default_dof, 1.0); + BOOST_CHECK_THROW(sva_with_joint_limits(k_sva_two_joints, six, six), std::invalid_argument); + + const auto one = vec({1.0}); + BOOST_CHECK_THROW(sva_with_joint_limits(k_sva_two_joints, one, one), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(test_velocity_and_acceleration_lengths_must_agree) { + BOOST_CHECK_THROW(sva_with_joint_limits(k_sva_two_joints, vec({1.0, 1.0}), vec({1.0})), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(test_non_sva_and_malformed_documents_throw) { + const auto limits = vec({1.0, 1.0}); + + BOOST_CHECK_THROW(sva_with_joint_limits("{ not json", limits, limits), std::invalid_argument); + + constexpr char urdf_ish[] = R"({"kinematic_param_type": "URDF", "joints": []})"; + BOOST_CHECK_THROW(sva_with_joint_limits(urdf_ish, limits, limits), std::invalid_argument); + + constexpr char no_joints[] = R"({"kinematic_param_type": "SVA", "links": []})"; + BOOST_CHECK_THROW(sva_with_joint_limits(no_joints, limits, limits), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(test_zero_limits_are_written_not_dropped) { + // Zero is a real limit of zero on the wire, not a way of saying unbounded, so it has to be + // written. Yaskawa's own config validation rejects a zero element, but the schema permits one + // and dropping it would describe an axis that cannot move as unbounded. + const Json::Value patched = parse(sva_with_joint_limits(k_sva_two_joints, vec({0.0, 1.0}), vec({1.0, 0.0}))); + + BOOST_REQUIRE(patched["joints"][0].isMember("max_velocity")); + BOOST_CHECK_EQUAL(patched["joints"][0]["max_velocity"].asDouble(), 0.0); + BOOST_REQUIRE(patched["joints"][1].isMember("max_acceleration")); + BOOST_CHECK_EQUAL(patched["joints"][1]["max_acceleration"].asDouble(), 0.0); +} + +BOOST_AUTO_TEST_CASE(test_negative_limits_are_refused) { + // Unlike zero, a negative limit is not a limit. Config validation rejects one before it could + // reach us, so this only fires if that stops being true or a caller builds the vectors itself. + BOOST_CHECK_THROW(sva_with_joint_limits(k_sva_two_joints, vec({1.0, -1.0}), vec({1.0, 1.0})), std::invalid_argument); + BOOST_CHECK_THROW(sva_with_joint_limits(k_sva_two_joints, vec({1.0, 1.0}), vec({-1.0, 1.0})), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(test_absent_param_type_is_treated_as_sva) { + // RDK reads a missing `kinematic_param_type` as SVA (referenceframe/model_json.go), so we have + // to as well, or a shipped file that omits it loses its limits. + constexpr char no_param_type[] = R"({ + "joints": [ + {"id": "joint_1", "type": "revolute", "min": -170.0, "max": 170.0} + ] + })"; + + const auto limits = vec({degrees_to_radians(90.0)}); + const Json::Value patched = parse(sva_with_joint_limits(no_param_type, limits, limits)); + BOOST_CHECK_CLOSE(patched["joints"][0]["max_velocity"].asDouble(), 90.0, 1e-9); +} + +BOOST_AUTO_TEST_CASE(test_non_revolute_and_mimic_joints_are_refused) { + const auto limits = vec({degrees_to_radians(90.0)}); + + // RDK does not convert `max_velocity` for a prismatic joint, so a radian value would be read as + // millimetres per second. We have no mm/s to offer from a `speed_rad_per_sec` config. + constexpr char prismatic[] = R"({ + "kinematic_param_type": "SVA", + "joints": [{"id": "rail", "type": "prismatic", "min": 0.0, "max": 500.0}] + })"; + BOOST_CHECK_THROW(sva_with_joint_limits(prismatic, limits, limits), std::invalid_argument); + + // A mimic joint carrying its own limits makes RDK reject the entire model with + // ErrMimicWithLimits, so publishing one would be worse than publishing no limits at all. + constexpr char mimic[] = R"({ + "kinematic_param_type": "SVA", + "joints": [{"id": "follower", "type": "revolute", "mimic": {"source": "joint_1"}}] + })"; + BOOST_CHECK_THROW(sva_with_joint_limits(mimic, limits, limits), std::invalid_argument); + + // A joint with no `type` is not something RDK can build a frame from either. + constexpr char untyped[] = R"({ + "kinematic_param_type": "SVA", + "joints": [{"id": "mystery", "min": -1.0, "max": 1.0}] + })"; + BOOST_CHECK_THROW(sva_with_joint_limits(untyped, limits, limits), std::invalid_argument); +} + +BOOST_AUTO_TEST_CASE(test_every_shipped_model_patches_at_the_default_dof) { + // A default config gives us k_default_dof limits, so every shipped document has to have that + // many joints or get_kinematics throws for that model. This is the test that fails when a new + // model file arrives with a different joint count. + const auto limits = Eigen::VectorXd::Constant(k_default_dof, degrees_to_radians(90.0)); + + const std::filesystem::path kinematics_dir{VIAM_YASKAWA_TEST_KINEMATICS_DIR}; + std::vector shipped; + for (const auto& entry : std::filesystem::directory_iterator{kinematics_dir}) { + if (entry.path().extension() == ".json") { + shipped.push_back(entry.path()); + } + } + BOOST_REQUIRE_MESSAGE(!shipped.empty(), "no shipped kinematics files found in " + kinematics_dir.string()); + + for (const auto& path : shipped) { + std::ifstream in{path}; + BOOST_REQUIRE_MESSAGE(in, "unable to open " + path.string()); + std::ostringstream buffer; + buffer << in.rdbuf(); + const std::string text = buffer.str(); + + BOOST_TEST_CONTEXT(path.filename().string()) { + std::string patched_text; + BOOST_REQUIRE_NO_THROW(patched_text = sva_with_joint_limits(text, limits, limits)); + + const Json::Value patched = parse(patched_text); + BOOST_REQUIRE_EQUAL(patched["joints"].size(), static_cast(k_default_dof)); + for (Json::ArrayIndex i = 0; i < patched["joints"].size(); ++i) { + BOOST_CHECK_CLOSE(patched["joints"][i]["max_velocity"].asDouble(), 90.0, 1e-9); + BOOST_CHECK_CLOSE(patched["joints"][i]["max_acceleration"].asDouble(), 90.0, 1e-9); + } + } + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/src/viam/module/utils.cpp b/src/viam/module/utils.cpp index 56837be..9ce681f 100644 --- a/src/viam/module/utils.cpp +++ b/src/viam/module/utils.cpp @@ -10,6 +10,8 @@ #include #include +#include + #include #include #include @@ -173,6 +175,82 @@ Eigen::VectorXd read_limit_vector(const viam::sdk::ResourceConfig& config, const return result; } +std::string sva_with_joint_limits(const std::string& sva_json, + const Eigen::VectorXd& velocity_rad_per_sec, + const Eigen::VectorXd& acceleration_rad_per_sec2) { + if (velocity_rad_per_sec.size() != acceleration_rad_per_sec2.size()) { + throw std::invalid_argument(std::format("velocity limits ({} joints) and acceleration limits ({} joints) must be the same length", + velocity_rad_per_sec.size(), + acceleration_rad_per_sec2.size())); + } + + Json::Value root; + { + std::istringstream in{sva_json}; + const Json::CharReaderBuilder reader_builder; + std::string errs; + if (!Json::parseFromStream(reader_builder, in, &root, &errs)) { + throw std::invalid_argument(std::format("kinematics document is not valid JSON: {}", errs)); + } + } + + // We only know how to edit SVA. A URDF-backed document has no `joints` array to carry these + // fields, and converting it here would mean reimplementing RDK's URDF parser. An absent + // `kinematic_param_type` means SVA, matching how RDK reads it in `referenceframe/model_json.go`. + const std::string param_type = root.get("kinematic_param_type", "").asString(); + if (!param_type.empty() && param_type != "SVA") { + throw std::invalid_argument(std::format("kinematics document is `{}`, not SVA, so it cannot carry joint limits", param_type)); + } + if (!root.isMember("joints") || !root["joints"].isArray()) { + throw std::invalid_argument("kinematics document has no `joints` array"); + } + + // This also catches limits we never populated, since a default-constructed Eigen vector is + // empty rather than a run of zeros, and zero would have been published as a real limit. + Json::Value& joints = root["joints"]; + if (static_cast(joints.size()) != velocity_rad_per_sec.size()) { + throw std::invalid_argument(std::format( + "kinematics document has {} joints but {} joint limits were configured", joints.size(), velocity_rad_per_sec.size())); + } + + for (Json::ArrayIndex i = 0; i < joints.size(); ++i) { + // Our limits are positional and in radians, which only means something for an independent + // revolute joint. A prismatic joint wants mm/s and RDK does not convert what we write, so a + // radian value would be read as millimeters. A mimic joint takes its limits from its source + // and RDK rejects the whole model if one carries limits of its own. We have no correct value + // for either, and we would rather fail than publish a document that is quietly wrong. + const std::string type = joints[i].get("type", "").asString(); + if (type != "revolute") { + throw std::invalid_argument(std::format("joint `{}` is `{}`, and joint limits are only defined here for revolute joints", + joints[i].get("id", "?").asString(), + type)); + } + if (joints[i].isMember("mimic")) { + throw std::invalid_argument( + std::format("joint `{}` is a mimic joint, which must not carry its own limits", joints[i].get("id", "?").asString())); + } + + // Zero we write, since only an absent field means unbounded and a zero limit is a real one + // saying the joint does not move. Negative is not a limit at all. Config validation already + // rejects it, but we check here too rather than leave this function correct only for as + // long as that stays true. + const auto joint = static_cast(i); + if (velocity_rad_per_sec[joint] < 0.0 || acceleration_rad_per_sec2[joint] < 0.0) { + throw std::invalid_argument(std::format("joint `{}` was given a negative limit ({} rad/s, {} rad/s²)", + joints[i].get("id", "?").asString(), + velocity_rad_per_sec[joint], + acceleration_rad_per_sec2[joint])); + } + + joints[i]["max_velocity"] = radians_to_degrees(velocity_rad_per_sec[joint]); + joints[i]["max_acceleration"] = radians_to_degrees(acceleration_rad_per_sec2[joint]); + } + + Json::StreamWriterBuilder writer_builder; + writer_builder["indentation"] = " "; + return Json::writeString(writer_builder, root); +} + void apply_move_limit(Eigen::VectorXd& limits, const boost::variant>& value) { struct visitor { Eigen::VectorXd& limits; diff --git a/src/viam/module/utils.hpp b/src/viam/module/utils.hpp index b1ea22a..b3b981e 100644 --- a/src/viam/module/utils.hpp +++ b/src/viam/module/utils.hpp @@ -131,6 +131,22 @@ Eigen::Index number_of_dof_configured(const viam::sdk::ResourceConfig& config, c // Scalars are broadcast to target_dof elements. Arrays must match target_dof exactly. Eigen::VectorXd read_limit_vector(const viam::sdk::ResourceConfig& config, const std::string& attribute, Eigen::Index target_dof); +// Returns the SVA kinematics document with per-joint `max_velocity` and `max_acceleration` added, +// so the motion service can see what this arm instance will actually honor rather than position +// bounds alone. The limits are in radians, matching what we read from config, and are written as +// the degrees the SVA schema uses. +// +// The two vectors are indexed in the document's own `joints` order, since the config gives us +// positional arrays and no joint names. They must both be exactly as long as that array, and a +// mismatch throws rather than patching the joints we happen to have values for, because a config +// whose DOF disagrees with the model is not describing this arm. +// +// Throws std::invalid_argument if the document is not parseable, is not SVA, has no `joints` +// array, or if either vector's length disagrees with the joint count. +std::string sva_with_joint_limits(const std::string& sva_json, + const Eigen::VectorXd& velocity_rad_per_sec, + const Eigen::VectorXd& acceleration_rad_per_sec2); + // Converts an sdk trajectory_point into the trajectory_point_t we put on the wire. // // The sdk gives us every joint value in degrees and the time as a microsecond offset from the diff --git a/src/viam/module/yaskawa_arm.cpp b/src/viam/module/yaskawa_arm.cpp index 51a319b..0711c04 100644 --- a/src/viam/module/yaskawa_arm.cpp +++ b/src/viam/module/yaskawa_arm.cpp @@ -688,8 +688,23 @@ ::viam::sdk::KinematicsData YaskawaArm::get_kinematics(const ProtoStruct&) { throw std::runtime_error(boost::str(boost::format("error reading kinematics file '%1%'") % sva_file_path.string())); } - // Convert to unsigned char vector - return KinematicsDataSVA({temp_bytes.begin(), temp_bytes.end()}); + // The shipped file carries position bounds only, so we add the configured speed and + // acceleration on the way out. A MoveOptions override applies to one move and never touches + // these members, so what we publish is what an un-overridden move will use. + // + // RDK builds the machine's whole frame system from this call, so failing to attach limits must + // not fail the call. `validate_config_` accepts a configured DOF that disagrees with the + // model's joint count (a scalar paired with an array of another length, for instance), and that + // used to be harmless here because we returned the file untouched. We keep that behaviour and + // warn, since losing the limits is what every caller got before this existed. + try { + const auto patched = sva_with_joint_limits({temp_bytes.begin(), temp_bytes.end()}, velocity_limits_, acceleration_limits_); + return KinematicsDataSVA({patched.begin(), patched.end()}); + } catch (const std::exception& e) { + VIAM_SDK_LOG(warn) << "get_kinematics: serving '" << sva_file_path.string() + << "' without velocity or acceleration limits: " << e.what(); + return KinematicsDataSVA({temp_bytes.begin(), temp_bytes.end()}); + } } pose YaskawaArm::get_end_position(const ProtoStruct&) {