RSDK-14385: publish configured joint velocity and acceleration limits

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

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

src/viam/module/test/sva_joint_limits_test.cpp

@@ -0,0 +1,208 @@
+#define BOOST_TEST_MODULE SvaJointLimitsTest
+#include <boost/test/unit_test.hpp>
+
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include <json/json.h>
+
+#include <Eigen/Dense>
+#include <viam/lib/robot_socket.hpp>
+
+#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<double> values) {
+    return Eigen::VectorXd::Map(values.data(), static_cast<Eigen::Index>(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<std::filesystem::path> 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<Json::ArrayIndex>(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()

src/viam/module/utils.cpp

@@ -10,6 +10,8 @@
 #include <boost/numeric/conversion/cast.hpp>
 #include <boost/variant.hpp>
 
+#include <json/json.h>
+
 #include <Eigen/Dense>
 #include <viam/lib/robot_socket.hpp>
 #include <viam/sdk/common/proto_value.hpp>
@@ -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<Eigen::Index>(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<Eigen::Index>(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<double, std::vector<double>>& value) {
     struct visitor {
         Eigen::VectorXd& limits;

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

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&) {