[RSDK-14385] Emit per-joint velocity and acceleration limits

src/viam/ur/module/test.cpp

@@ -1274,11 +1274,12 @@ BOOST_AUTO_TEST_CASE(test_apply_calibration_preserves_world_geometry_centers_at_
 }
 
 BOOST_AUTO_TEST_CASE(test_to_sva_json_round_trips_via_parse) {
-    // parse -> apply_calibration(nominal) -> to_sva_json -> re-parse
-    // should give back world-frame geometry positions that still match the
-    // spec. Exercises the writer (translation/orientation/geometry
-    // emission) and the parser's quaternion code path on the writer's
-    // output.
+    // parse -> apply_calibration(nominal) -> apply_kinematic_limits ->
+    // to_sva_json -> re-parse should give back world-frame geometry positions
+    // that still match the spec, and the stamped velocity and acceleration
+    // limits in degrees. Exercises the writer (translation/orientation/
+    // geometry/limit emission) and the parser's quaternion code path on the
+    // writer's output.
     const UrArmModel::Kinematics tbl = load("ur20");
 
     DHParams dh{};
@@ -1287,7 +1288,14 @@ BOOST_AUTO_TEST_CASE(test_to_sva_json_round_trips_via_parse) {
     dh.alpha = {std::numbers::pi / 2.0, 0.0, 0.0, std::numbers::pi / 2.0, -std::numbers::pi / 2.0, 0.0};
     dh.theta = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
-    const std::string json_str = tbl.apply_calibration(dh).to_sva_json();
+    // Distinct per joint, so a fanned-out or transposed write fails here
+    // rather than passing on six equal values.
+    const vector6d_t velocity_degs = {110.0, 120.0, 130.0, 140.0, 150.0, 160.0};
+    const vector6d_t acceleration_degs = {210.0, 220.0, 230.0, 240.0, 250.0, 260.0};
+
+    const std::string json_str = tbl.apply_calibration(dh)
+                                     .apply_kinematic_limits(degrees_to_radians(velocity_degs), degrees_to_radians(acceleration_degs))
+                                     .to_sva_json();
     const auto tmp = std::filesystem::temp_directory_path() / "ur20_round_trip.json";
     {
         std::ofstream out(tmp);
@@ -1310,6 +1318,120 @@ BOOST_AUTO_TEST_CASE(test_to_sva_json_round_trips_via_parse) {
         BOOST_CHECK_SMALL(world.y() - expect[1], 1e-3);
         BOOST_CHECK_SMALL(world.z() - expect[2], 1e-3);
     }
+
+    // The stamping input is radians and the schema is degrees, so this also
+    // covers the conversion.
+    for (std::size_t i = 0; i < 6; ++i) {
+        BOOST_REQUIRE(reparsed.limits[i].max_velocity_deg_per_sec.has_value());
+        BOOST_REQUIRE(reparsed.limits[i].max_acceleration_deg_per_sec2.has_value());
+        BOOST_CHECK_CLOSE(*reparsed.limits[i].max_velocity_deg_per_sec, velocity_degs[i], 1e-9);
+        BOOST_CHECK_CLOSE(*reparsed.limits[i].max_acceleration_deg_per_sec2, acceleration_degs[i], 1e-9);
+    }
+
+    // Position bounds must survive the added fields untouched.
+    for (std::size_t i = 0; i < 6; ++i) {
+        BOOST_CHECK_EQUAL(reparsed.limits[i].min_deg, (i == 2) ? -180.0 : -360.0);
+        BOOST_CHECK_EQUAL(reparsed.limits[i].max_deg, (i == 2) ? 180.0 : 360.0);
+    }
+}
+
+BOOST_AUTO_TEST_CASE(test_configured_zero_limits_are_published_as_zero) {
+    // `speed_degs_per_sec` validation rejects an all-zero array but accepts a single zero element,
+    // and zero is a real limit of zero on the wire rather than a way of saying unbounded. So a
+    // configured zero has to be emitted, not dropped: dropping it would describe an axis that
+    // cannot move as having no bound at all.
+    const vector6d_t velocity_degs = {110.0, 0.0, 130.0, 140.0, 150.0, 160.0};
+    const vector6d_t acceleration_degs = {210.0, 220.0, 0.0, 240.0, 250.0, 260.0};
+
+    const auto stamped = load("ur20").apply_kinematic_limits(degrees_to_radians(velocity_degs), degrees_to_radians(acceleration_degs));
+
+    BOOST_REQUIRE(stamped.limits[1].max_velocity_deg_per_sec.has_value());
+    BOOST_CHECK_EQUAL(*stamped.limits[1].max_velocity_deg_per_sec, 0.0);
+    BOOST_REQUIRE(stamped.limits[2].max_acceleration_deg_per_sec2.has_value());
+    BOOST_CHECK_EQUAL(*stamped.limits[2].max_acceleration_deg_per_sec2, 0.0);
+
+    // The other axis of the same joint is untouched.
+    BOOST_REQUIRE(stamped.limits[1].max_acceleration_deg_per_sec2.has_value());
+    BOOST_CHECK_CLOSE(*stamped.limits[1].max_acceleration_deg_per_sec2, 220.0, 1e-9);
+
+    // And the key is present in the document with an explicit 0, not missing.
+    Json::Value root;
+    std::istringstream in{stamped.to_sva_json()};
+    const Json::CharReaderBuilder reader_builder;
+    std::string errs;
+    BOOST_REQUIRE(Json::parseFromStream(reader_builder, in, &root, &errs));
+    BOOST_REQUIRE(root["joints"][1].isMember("max_velocity"));
+    BOOST_CHECK_EQUAL(root["joints"][1]["max_velocity"].asDouble(), 0.0);
+    BOOST_REQUIRE(root["joints"][2].isMember("max_acceleration"));
+    BOOST_CHECK_EQUAL(root["joints"][2]["max_acceleration"].asDouble(), 0.0);
+}
+
+BOOST_AUTO_TEST_CASE(test_serializing_without_limits_is_refused) {
+    // The shipped files carry position bounds only, so a freshly parsed table has no kinematic
+    // limits. Publishing it in that state would hand RDK a document whose `TrajectoryLimits` comes
+    // back false and silently costs the whole arm its timing, so serializing has to fail loudly
+    // instead.
+    const auto tbl = load("ur20");
+    for (std::size_t i = 0; i < 6; ++i) {
+        BOOST_CHECK(!tbl.limits[i].max_velocity_deg_per_sec.has_value());
+        BOOST_CHECK(!tbl.limits[i].max_acceleration_deg_per_sec2.has_value());
+    }
+
+    BOOST_CHECK_THROW(tbl.to_sva_json(), std::logic_error);
+
+    // One joint short is refused for the same reason: RDK's check is all or nothing, so a document
+    // missing a single limit is worth no more than one missing every limit.
+    auto partial = tbl.apply_kinematic_limits(degrees_to_radians(vector6d_t{110.0, 120.0, 130.0, 140.0, 150.0, 160.0}),
+                                              degrees_to_radians(vector6d_t{210.0, 220.0, 230.0, 240.0, 250.0, 260.0}));
+    BOOST_CHECK_NO_THROW(partial.to_sva_json());
+
+    // Both sides of the guard, since a document missing only accelerations is refused by RDK just
+    // as flatly as one missing velocities.
+    auto no_velocity = partial;
+    no_velocity.limits[3].max_velocity_deg_per_sec.reset();
+    BOOST_CHECK_THROW(no_velocity.to_sva_json(), std::logic_error);
+
+    auto no_acceleration = partial;
+    no_acceleration.limits[3].max_acceleration_deg_per_sec2.reset();
+    BOOST_CHECK_THROW(no_acceleration.to_sva_json(), std::logic_error);
+}
+
+BOOST_AUTO_TEST_CASE(test_malformed_kinematic_limits_are_rejected_on_parse) {
+    // Nothing in production reaches these branches, because every emitted document is stamped and
+    // no shipped file carries the fields at all. They exist so the parser stands on its own rather
+    // than relying on `apply_kinematic_limits` overwriting whatever it read.
+    const auto write_and_load = [](const std::string& joint_patch) {
+        const auto tmp = std::filesystem::temp_directory_path() / "ur20_bad_limits.json";
+        {
+            // Start from a real document so only the field under test is unusual.
+            auto doc = load("ur20").apply_kinematic_limits(degrees_to_radians(vector6d_t{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}),
+                                                           degrees_to_radians(vector6d_t{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}));
+            Json::Value root;
+            std::istringstream in{doc.to_sva_json()};
+            const Json::CharReaderBuilder reader_builder;
+            std::string errs;
+            BOOST_REQUIRE(Json::parseFromStream(reader_builder, in, &root, &errs));
+
+            Json::Value patch;
+            std::istringstream patch_in{joint_patch};
+            BOOST_REQUIRE(Json::parseFromStream(reader_builder, patch_in, &patch, &errs));
+            for (const auto& key : patch.getMemberNames()) {
+                root["joints"][0][key] = patch[key];
+            }
+
+            std::ofstream out(tmp);
+            out << Json::writeString(Json::StreamWriterBuilder{}, root);
+        }
+        return UrArmModel::from_sdk_name("ur20").load_kinematics(tmp);
+    };
+
+    BOOST_CHECK_THROW(write_and_load(R"({"max_velocity": "fast"})"), std::invalid_argument);
+    BOOST_CHECK_THROW(write_and_load(R"({"max_acceleration": "quick"})"), std::invalid_argument);
+    BOOST_CHECK_THROW(write_and_load(R"({"max_velocity": -180.0})"), std::invalid_argument);
+    BOOST_CHECK_THROW(write_and_load(R"({"max_acceleration": -1145.0})"), std::invalid_argument);
+
+    // Zero stays acceptable on the way in, since it is a real limit.
+    BOOST_CHECK_NO_THROW(write_and_load(R"({"max_velocity": 0.0})"));
 }
 
 BOOST_AUTO_TEST_SUITE_END()

src/viam/ur/module/ur_arm_model.cpp

@@ -6,6 +6,7 @@
 #include <fstream>
 #include <initializer_list>
 #include <memory>
+#include <optional>
 #include <stdexcept>
 #include <string>
 #include <type_traits>
@@ -253,7 +254,30 @@ JointLimits parse_joint_limits(const std::filesystem::path& path, const Json::Va
     if (!joint.isMember("max") || !joint["max"].isNumeric()) {
         throw_parse_error(path, "joint missing numeric `max`");
     }
-    return JointLimits{joint["min"].asDouble(), joint["max"].asDouble()};
+
+    // `max_velocity` and `max_acceleration` are optional in the schema, so an
+    // absent field is not an error, but a present one that is not a number is.
+    // We read them so a document we emitted parses back to what we wrote.
+    //
+    // Negative is rejected for the same reason config parsing rejects it: there is no such thing
+    // as a negative speed limit. Zero is allowed, since that is a real limit meaning the joint does
+    // not move. Unlike `min` and `max`, which are positions and are routinely negative.
+    const auto optional_limit = [&](const char* field) -> std::optional<double> {
+        if (!joint.isMember(field)) {
+            return std::nullopt;
+        }
+        if (!joint[field].isNumeric()) {
+            throw_parse_error(path, std::string{"joint `"} + field + "` is present but not a number");
+        }
+        const double value = joint[field].asDouble();
+        if (value < 0.0) {
+            throw_parse_error(path, std::string{"joint `"} + field + "` cannot be negative, got " + std::to_string(value));
+        }
+        return value;
+    };
+
+    return JointLimits{
+        joint["min"].asDouble(), joint["max"].asDouble(), optional_limit("max_velocity"), optional_limit("max_acceleration")};
 }
 
 Json::Value translation_json(const Eigen::Vector3d& t) {
@@ -399,6 +423,19 @@ UrArmModel::Kinematics UrArmModel::Kinematics::apply_calibration(const DHParams&
     return out;
 }
 
+UrArmModel::Kinematics UrArmModel::Kinematics::apply_kinematic_limits(const urcl::vector6d_t& velocity_rad_per_sec,
+                                                                      const urcl::vector6d_t& acceleration_rad_per_sec2) const {
+    // We publish whatever is configured, zero included. Only an absent field means unbounded, so a
+    // configured zero has to go out as zero: our validation accepts an array with a single zero
+    // element, and omitting it would tell the planner that an axis which cannot move is unbounded.
+    Kinematics out = *this;
+    for (std::size_t i = 0; i < k_num_dh_joints; ++i) {
+        out.limits[i].max_velocity_deg_per_sec = radians_to_degrees(velocity_rad_per_sec[i]);
+        out.limits[i].max_acceleration_deg_per_sec2 = radians_to_degrees(acceleration_rad_per_sec2[i]);
+    }
+    return out;
+}
+
 Eigen::Matrix4d UrArmModel::Kinematics::parent_pose_at(std::size_t i) const {
     Eigen::Matrix4d acc = Eigen::Matrix4d::Identity();
     for (std::size_t k = 0; k < i; ++k) {
@@ -566,6 +603,18 @@ std::string UrArmModel::Kinematics::to_sva_json() const {
             joint["axis"] = dh_z_axis;
             joint["min"] = limits[i - 1].min_deg;
             joint["max"] = limits[i - 1].max_deg;
+
+            // Every joint we publish has to carry both limits. A UR joint always has a real speed
+            // the motors can do, so there is no such thing as an unbounded one here, and RDK's
+            // `TrajectoryLimits` is all or nothing anyway: leave one joint unset and the planner
+            // discards timing for the whole arm. Emitting a partial document would be a silent
+            // no-op at the far end, so we refuse rather than serialize one.
+            if (!limits[i - 1].max_velocity_deg_per_sec || !limits[i - 1].max_acceleration_deg_per_sec2) {
+                throw std::logic_error("UrArmModel::Kinematics::to_sva_json: joint " + std::to_string(i - 1) +
+                                       " has no velocity or acceleration limit; call apply_kinematic_limits first");
+            }
+            joint["max_velocity"] = *limits[i - 1].max_velocity_deg_per_sec;
+            joint["max_acceleration"] = *limits[i - 1].max_acceleration_deg_per_sec2;
             joints.append(joint);
         }
 

src/viam/ur/module/ur_arm_model.hpp

@@ -43,11 +43,18 @@ struct DHParams {
     urcl::vector6d_t theta;
 };
 
-// Joint angular limits, in degrees, matching the `min`/`max` fields in
-// shipped `kinematics/<model>.json` files.
+// Joint limits, in degrees, matching the `min`/`max`/`max_velocity`/
+// `max_acceleration` fields in shipped `kinematics/<model>.json` files.
+//
+// The kinematic limits are optional because the shipped files carry position
+// bounds only, and because zero is a real limit meaning the joint does not
+// move, so it cannot double as "not set". `apply_kinematic_limits` fills them
+// in per arm instance and `to_sva_json` refuses to serialize until it has.
 struct JointLimits {
     double min_deg;
     double max_deg;
+    std::optional<double> max_velocity_deg_per_sec;
+    std::optional<double> max_acceleration_deg_per_sec2;
 };
 
 // A geometry expressed in its link's parent (joint) frame.
@@ -125,7 +132,15 @@ class UrArmModel::Kinematics {
 
     Kinematics apply_calibration(const DHParams& dh) const;
 
-    // Serialize this kinematics to an RDK-compatible SVA kinematics JSON.
+    // Returns a copy carrying the given per-joint velocity and acceleration
+    // limits. Inputs are radians, matching what `URArm::state_` holds; the
+    // conversion to the degrees the SVA schema uses happens here.
+    Kinematics apply_kinematic_limits(const urcl::vector6d_t& velocity_rad_per_sec,
+                                      const urcl::vector6d_t& acceleration_rad_per_sec2) const;
+
+    // Serialize this kinematics to an RDK-compatible SVA kinematics JSON. Throws std::logic_error
+    // if any joint is missing a velocity or acceleration limit, so call `apply_kinematic_limits`
+    // first.
     std::string to_sva_json() const;
 
     // Cumulative world-frame pose of the i-th link's parent at joints with zero rotation.

src/viam/ur/module/ur_arm_state.cpp

@@ -209,8 +209,11 @@ std::unique_ptr<URArm::state_> URArm::state_::create(UrArmModel configured_model
                                           traceid_metadata_key,
                                           ports);
 
-    state->set_velocity_limits(parse_and_validate_joint_limits(config, "speed_degs_per_sec"));
-    state->set_acceleration_limits(parse_and_validate_joint_limits(config, "acceleration_degs_per_sec2"));
+    // The setters return what they actually stored, which is the configured value clamped against
+    // any ceiling, so we keep that as the configured limits the kinematics document publishes.
+    state->configured_velocity_limits_ = state->set_velocity_limits(parse_and_validate_joint_limits(config, "speed_degs_per_sec"));
+    state->configured_acceleration_limits_ =
+        state->set_acceleration_limits(parse_and_validate_joint_limits(config, "acceleration_degs_per_sec2"));
 
     // Hold the mutex while we start the worker thread. It will not be
     // able to advance, but will be ready to take over work as soon as
@@ -949,6 +952,13 @@ std::string URArm::state_::get_dh_kinematics_json(std::chrono::steady_clock::dur
     }
     const auto& payload = fut.get();
 
+    // A snapshot that was never taken would otherwise publish six joints at zero, which is a real
+    // limit meaning the arm cannot move, so it has to be caught here rather than sailing through
+    // as if it were configured that way.
+    if (!configured_velocity_limits_ || !configured_acceleration_limits_) {
+        throw std::logic_error("get_dh_kinematics_json: configured limits were never captured during create");
+    }
+
     // The first JSON-wanting caller builds the string under `call_once`;
     // subsequent callers reuse the memoized value. `json_once`/`json` are
     // `mutable` on `cached_kinematics_payload` precisely so this lazy build
@@ -958,6 +968,7 @@ std::string URArm::state_::get_dh_kinematics_json(std::chrono::steady_clock::dur
     std::call_once(*payload.json_once, [&] {
         payload.json = payload.arm_model.load_kinematics((resource_root_ / "kinematics" / payload.arm_model.sdk_name()).concat(".json"))
                            .apply_calibration({payload.info.dh_a_, payload.info.dh_d_, payload.info.dh_alpha_, payload.info.dh_theta_})
+                           .apply_kinematic_limits(*configured_velocity_limits_, *configured_acceleration_limits_)
                            .to_sva_json();
     });
     return payload.json;

src/viam/ur/module/ur_arm_state.hpp

@@ -266,9 +266,13 @@ class URArm::state_ {
     //
     // `mutable` is intentional on `json_once`/`json`: the shared state is
     // accessed via `shared_future<T>::get()` returning `const T&`, and the
-    // JSON is a deterministic function of `info` and `arm_model`. Every
-    // caller observes the same logical value; `std::call_once` synchronizes
-    // the first JSON-wanting caller's build with later reuses.
+    // JSON is a deterministic function of this payload's `info` and
+    // `arm_model` plus the configured limits, none of which change while the
+    // payload lives. Every caller observes the same logical value;
+    // `std::call_once` synchronizes the first JSON-wanting caller's build
+    // with later reuses. Calibration is per connection rather than per
+    // `state_`, so the once_flag has to live here; hoisting it would keep
+    // serving the previous connection's calibration after a reconnect.
     //
     // `json_once` is held via `unique_ptr` because `std::once_flag` is
     // neither copyable nor movable, and the payload must be at least
@@ -594,6 +598,20 @@ class URArm::state_ {
     vector6d_t velocity_limits_{};
     vector6d_t acceleration_limits_{};
 
+    // The configured limits, already clamped against any ceiling, captured once during `create`.
+    // The kinematics document describes how this arm is configured, so it publishes these rather
+    // than the live `velocity_limits_`, which DoCommand moves for the session and MoveOptions moves
+    // for a single move. That means the document is not an upper bound on what the arm will do; see
+    // the pull request for why we chose it anyway.
+    //
+    // Optional because a default-constructed vector is all zeros, and zero is a real limit meaning
+    // the joint does not move, so an unpopulated snapshot would be published as if it were an arm
+    // configured to stay still.
+    //
+    // Written before the worker thread starts, so no lock is needed to read them.
+    std::optional<vector6d_t> configured_velocity_limits_;
+    std::optional<vector6d_t> configured_acceleration_limits_;
+
     const double path_tolerance_delta_rads_;
     const std::optional<double> path_colinearization_ratio_;
     const double segmentation_threshold_;