diff --git a/src/viam/ur/module/test.cpp b/src/viam/ur/module/test.cpp index 31c93035..38c028bc 100644 --- a/src/viam/ur/module/test.cpp +++ b/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() diff --git a/src/viam/ur/module/ur_arm_model.cpp b/src/viam/ur/module/ur_arm_model.cpp index 86321189..b5cd5314 100644 --- a/src/viam/ur/module/ur_arm_model.cpp +++ b/src/viam/ur/module/ur_arm_model.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -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 { + 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); } diff --git a/src/viam/ur/module/ur_arm_model.hpp b/src/viam/ur/module/ur_arm_model.hpp index 19db4470..a80b9ef6 100644 --- a/src/viam/ur/module/ur_arm_model.hpp +++ b/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/.json` files. +// Joint limits, in degrees, matching the `min`/`max`/`max_velocity`/ +// `max_acceleration` fields in shipped `kinematics/.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 max_velocity_deg_per_sec; + std::optional 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. diff --git a/src/viam/ur/module/ur_arm_state.cpp b/src/viam/ur/module/ur_arm_state.cpp index f047b26f..8f0d1c15 100644 --- a/src/viam/ur/module/ur_arm_state.cpp +++ b/src/viam/ur/module/ur_arm_state.cpp @@ -209,8 +209,11 @@ std::unique_ptr 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; diff --git a/src/viam/ur/module/ur_arm_state.hpp b/src/viam/ur/module/ur_arm_state.hpp index e10a9b94..80631932 100644 --- a/src/viam/ur/module/ur_arm_state.hpp +++ b/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::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 configured_velocity_limits_; + std::optional configured_acceleration_limits_; + const double path_tolerance_delta_rads_; const std::optional path_colinearization_ratio_; const double segmentation_threshold_;