diff --git a/include/litearm/arm.hpp b/include/litearm/arm.hpp index 993455e..1f3ed42 100644 --- a/include/litearm/arm.hpp +++ b/include/litearm/arm.hpp @@ -286,6 +286,17 @@ class Arm { /// 单发 move_js。连续伺服需调用方按 >=10Hz 重发, 否则 0.1s 看门狗 fail-soft。 void move_js(const std::vector& q, const std::vector& dq = {}, const std::vector& tau_ff = {}); + /// 从臂跟随 (0x08): PD + 固件侧前馈的关节阻抗。连续伺服需调用方按 >=10Hz 重发。 + /// + /// 逐轴给刚度 `kp` 与阻尼 `kd`; 载荷不带 tau —— 重力与关节限位墙的前馈由固件每拍算。 + /// `q` 仍被固件钳进关节限位, `kp`/`kd` 钳到固件侧的 MIT_KP_MAX / MIT_KD_MAX, `dq` 是 + /// **速度参考** (有符号) 而不是 move_js 里的斜坡上限。 + /// + /// ⚠ 本命令进入一个**会话**, 会话期间安全包络的超速与位置规则被豁免 —— **上位机是 + /// 唯一的界**。任何其它运动或停止命令退出会话, hold 期间也退出, 且主机静默过久会 + /// 让豁免失效。要走"不考虑限速的实时控制"就是这条, 代价也是这条。 + void joint_follow(const std::vector& q, const std::vector& dq, + const std::vector& kp, const std::vector& kd); /// 单关节 MIT 透传 (0x04)。⚠ 绕过规划; 须调用方维持 >=10Hz。 void send_mit(int idx, double q, double dq, double kp, double kd, double tau); /// 全臂单帧透传 (0x05)。⚠ 同上。 diff --git a/include/litearm/protocol.hpp b/include/litearm/protocol.hpp index 2ecd91b..58b2cec 100644 --- a/include/litearm/protocol.hpp +++ b/include/litearm/protocol.hpp @@ -34,6 +34,14 @@ inline constexpr uint8_t CMD_ZERO_G = 0x06; // [SYNC] 同步 PTP movej: 全关节同时到达, 末端沿关节空间直线 q(s) = q0 + s*dq。 // 载荷与 CMD_MOVE_J 逐字节相同; 语义差异只在轨迹形状。 inline constexpr uint8_t CMD_MOVE_J_SYNC = 0x07; +// [JOINT_FOLLOW] 从臂跟随: PD + 固件侧前馈的关节阻抗通路。 +// 载荷 q[N] + dq[N] + K[N] + B[N] = 16N 字节, **不带 tau** —— 前馈 (重力 + 关节限位墙) +// 由固件每拍算。q 仍被钳进关节限位, K/B 钳到 MIT_KP_MAX / MIT_KD_MAX。 +// +// ⚠ 与 move_js / move_mit_all 的关键差别: 本命令进入一个**会话** (固件侧 s_jf_ff), 会话 +// 期间安全包络的超速与位置规则被豁免, 所以**上位机是唯一的界**。会话由任何其它运动或停止 +// 命令退出, hold 期间也退出, 且主机静默过久会让豁免失效。 +inline constexpr uint8_t CMD_JOINT_FOLLOW = 0x08; inline constexpr uint8_t CMD_ENABLE = 0x10; inline constexpr uint8_t CMD_DISABLE = 0x11; inline constexpr uint8_t CMD_EMERGENCY_STOP = 0x12; diff --git a/src/arm.cpp b/src/arm.cpp index 98415ee..b584c30 100644 --- a/src/arm.cpp +++ b/src/arm.cpp @@ -1233,6 +1233,23 @@ void Arm::move_js(const std::vector& q, const std::vector& dq, cmd_expect_ack(proto::CMD_MOVE_JS, payload, "move_js"); } +void Arm::joint_follow(const std::vector& q, const std::vector& dq, + const std::vector& kp, const std::vector& kd) { + reject_if_in_dfu(); + // 四条都排在本 arity 预检里, 顺序与载荷顺序一致 —— 报出来的名字就是缺的那个数组。 + const std::vector*, const char*>> arrs = { + {&q, "q"}, {&dq, "dq"}, {&kp, "kp"}, {&kd, "kd"}}; + for (const auto& kv : arrs) { + if (int(kv.first->size()) != n_) { + throw InvalidCommandError(std::string("joint_follow ") + kv.second + " 需 N 个"); + } + } + std::vector all; + all.reserve(size_t(4 * n_)); + for (const auto& kv : arrs) all.insert(all.end(), kv.first->begin(), kv.first->end()); + cmd_expect_ack(proto::CMD_JOINT_FOLLOW, proto::pack_f32s(all), "joint_follow"); +} + void Arm::send_mit(int idx, double q, double dq, double kp, double kd, double tau) { reject_if_in_dfu(); // 排在 idx 预检之前 (终态下 n_=0 => 否则报"idx 越界") if (idx < 0 || idx >= n_) throw InvalidCommandError("idx 越界"); diff --git a/src/protocol.cpp b/src/protocol.cpp index 7270edb..6e6936a 100644 --- a/src/protocol.cpp +++ b/src/protocol.cpp @@ -18,12 +18,12 @@ const char* const kFlagNames[] = {"FAULT", "WD_TRIPPED", "FB_STALE", // 命令覆盖契约 —— 与 Python 侧 _protocol.COMMAND_COVERAGE 逐条一致。 const std::vector kCoverage = { {0x01, "Arm.movej"}, - {0x07, "Arm.movej_sync"}, - {0x02, "Arm.move_p"}, + {0x07, "Arm.movej_sync"}, {0x02, "Arm.move_p"}, {0x03, "Arm.move_js"}, {0x04, "Arm.send_mit"}, {0x05, "Arm.send_mit_all"}, {0x06, "Arm.zero_g"}, + {0x08, "Arm.joint_follow"}, {0x10, "Arm.enable"}, {0x11, "Arm.disable"}, {0x12, "Arm.emergency_stop"}, @@ -288,17 +288,13 @@ const std::map& firmware_only_rsps() { const std::map& preexisting_gaps() { // 已知缺口 —— **本应同步而未同步** (与 firmware_only_cmds 的"有意不做"不同)。 // - // `CMD_JOINT_FOLLOW` (0x08): 固件侧早已实现 (usb_cmd.c 的 case, 载荷 16N 字节 = - // q[N]+dq[N]+K[N]+B[N], 前馈由固件算), 而本 SDK 一直没有对应常量与入口 —— 不是 - // "有意不暴露", 是**确实还没做**。登记在此是为了让 - // `test_protocol_sync` 的两条判据如实反映现状 (缺口存在, 且它没有被伪装成 - // "固件单边命令"), 而不是宣布任何最终处置。 + // **当前为空**, 因为它唯一的一条已经补上: `CMD_JOINT_FOLLOW` (0x08) 的 SDK 入口 + // 见 `Arm::joint_follow`, 覆盖表里也有了它。登记那张表时的原话是"留在这里不是终点: + // 要么补上 SDK 入口, 要么裁决'有意不暴露'", 这里走的是第一条。 // - // ⚠ 留在这里**不是终点**: 要么补上 SDK 入口, 要么裁决"有意不暴露"并改登记到 - // firmware_only_cmds() —— 两条路都得有人拍板, 别让它无声地长住。 - static const std::map kGaps = { - {0x08, "CMD_JOINT_FOLLOW"}, - }; + // ⚠ 别顺手往这里塞东西: 空**是**"当前没有已知缺口"这句话的断言载体, 由 + // `tests/test_protocol.cpp` 钉住。真出现新缺口时加条目**等于宣布**它, 加之前先想清楚。 + static const std::map kGaps; return kGaps; } diff --git a/tests/test_commands.cpp b/tests/test_commands.cpp index 5540feb..f0a30e2 100644 --- a/tests/test_commands.cpp +++ b/tests/test_commands.cpp @@ -239,6 +239,41 @@ TEST(cmd_move_js_arity_and_payload) { } } +TEST(cmd_joint_follow_arity_and_payload_layout) { + lt::Offline off; + // 四个数组取值各不相同, 这样载荷里哪一段是谁才被真正测到: 固件按 q / dq / kp / kd 的 + // 顺序读, 错一位就是把刚度当成了目标位置 —— 而两者都是浮点数, 固件不会报错。 + const std::vector q(7, 0.1); + const std::vector dq(7, 0.2); + const std::vector kp(7, 30.0); + const std::vector kd(7, 1.5); + CHECK_NOTHROW(off->joint_follow(q, dq, kp, kd)); + + for (int missing = 0; missing < 4; ++missing) { + std::vector args[4] = {q, dq, kp, kd}; + args[missing] = std::vector(6, 0.0); + CHECK_THROWS_AS(off->joint_follow(args[0], args[1], args[2], args[3]), + InvalidCommandError); + } + + bool seen = false; + for (const auto& kv : off.t().tx_snapshot()) { + if (kv.first != proto::CMD_JOINT_FOLLOW) continue; + seen = true; + // q[N] + dq[N] + kp[N] + kd[N] = 16N 字节. **不带 tau**: 前馈由固件算。 + CHECK_EQ(kv.second.size(), size_t(16 * 7)); + const auto values = proto::unpack_f32s(kv.second.data(), 0, 28); + CHECK_EQ(int(values.size()), 28); + for (std::size_t i = 0; i < 7; ++i) { + CHECK_NEAR(values[i], 0.1, 1e-6); + CHECK_NEAR(values[7 + i], 0.2, 1e-6); + CHECK_NEAR(values[14 + i], 30.0, 1e-6); + CHECK_NEAR(values[21 + i], 1.5, 1e-6); + } + } + CHECK(seen); +} + TEST(cmd_send_mit_validates_index_and_payload_size) { lt::Offline off; CHECK_NOTHROW(off->send_mit(0, 0.1, 0.0, 30.0, 1.0, 0.0)); diff --git a/tests/test_full_coverage.cpp b/tests/test_full_coverage.cpp index 0985676..4636759 100644 --- a/tests/test_full_coverage.cpp +++ b/tests/test_full_coverage.cpp @@ -114,6 +114,8 @@ TEST(coverage_every_implemented_command_is_exercised_through_its_entry) { arm.send_mit(0, 0.0, 0.0, 50.0, 2.0, 0.0); // 0x04 arm.send_mit_all(zeros7(), zeros7(), std::vector(7, 50.0), std::vector(7, 2.0), zeros7()); // 0x05 + arm.joint_follow(zeros7(), zeros7(), std::vector(7, 30.0), + std::vector(7, 2.0)); // 0x08 arm.set_motion_mode(0); // 0x20 arm.set_speed(50); // 0x21 arm.ik({0.30, 0.0, 0.35, 0.0, 0.0, 0.0}); // 0x42 @@ -370,6 +372,7 @@ static const std::set kExercised = { "ff_scalar_items", "ff_scalar_ro_items", "ff_vec_items", "firmware", "fw_version", "get_ff_mask", "get_ff_scalar", "get_ff_vec", "get_state", "get_status_now", "get_tcp", "home", "host_stats", "ik", "is_connected", "is_in_dfu", + "joint_follow", "last_reset_reason", "license", "log", "model", "msg_hz", "options", "move_c", "move_js", "move_l", "move_p", "move_path", "movej", "movej_sync", diff --git a/tests/test_protocol.cpp b/tests/test_protocol.cpp index 36b4643..8a4957c 100644 --- a/tests/test_protocol.cpp +++ b/tests/test_protocol.cpp @@ -281,13 +281,14 @@ TEST(protocol_only_ack_and_err_are_keyed_by_echo) { TEST(protocol_command_coverage_matches_the_python_table_exactly) { // 判据是**逐条对表**, 不是"数量差不多": 这份契约是"固件每条已实现命令都有 SDK 入口" // 的断言载体, 少一条就是少一个入口。 - // ⚠ 数量是 49 —— 上游注释里那句"47/47"是更早一轮的记数, 表本身一直是 49 条。 + // ⚠ 数量是 50 —— 上游注释里那句"47/47"是更早一轮的记数。0x08 这一条原本登记在 + // preexisting_gaps() 里, 补上 Arm::joint_follow 之后进了这张表。 const std::set expected = { - 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x10, 0x11, 0x12, - 0x13, 0x14, 0x15, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, - 0x27, 0x28, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, 0x2F, 0x30, 0x31, - 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x3B, - 0x3C, 0x3D, 0x3E, 0x3F, 0x40, 0x41, 0x42, 0x43, 0x49, + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x10, 0x11, + 0x12, 0x13, 0x14, 0x15, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, + 0x26, 0x27, 0x28, 0x2A, 0x2B, 0x2C, 0x2D, 0x2E, 0x2F, 0x30, + 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, + 0x3B, 0x3C, 0x3D, 0x3E, 0x3F, 0x40, 0x41, 0x42, 0x43, 0x49, }; std::set seen; for (const auto& c : proto::command_coverage()) { @@ -311,10 +312,9 @@ TEST(protocol_sync_sentinel_tables_hold_exactly_their_declared_contents) { CHECK(proto::firmware_only_cmds().empty()); CHECK(proto::firmware_only_rsps().empty()); - // 已知缺口: CMD_JOINT_FOLLOW (0x08) 固件已实现、SDK 尚无入口。见 src/protocol.cpp。 - CHECK_EQ(int(proto::preexisting_gaps().size()), 1); - CHECK_EQ(int(proto::preexisting_gaps().count(0x08)), 1); - CHECK_EQ(proto::preexisting_gaps().at(0x08), std::string("CMD_JOINT_FOLLOW")); + // 缺口表为空 —— 它唯一的一条 (CMD_JOINT_FOLLOW, 0x08) 已经补上 SDK 入口并进了覆盖 + // 表, 见 src/protocol.cpp。空是"当前没有已知缺口"这句话的断言载体。 + CHECK(proto::preexisting_gaps().empty()); // ⚠ **本仓跑不了固件侧的比对**: 上游 `test_protocol_sync.py` 要解析固件仓的 // `hal/usb_cmd.h` 双向对表, 而这里没有固件树。所以**别把这条读成"已经跟固件对过账"**