Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions include/litearm/arm.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -286,6 +286,17 @@ class Arm {
/// 单发 move_js。连续伺服需调用方按 >=10Hz 重发, 否则 0.1s 看门狗 fail-soft。
void move_js(const std::vector<double>& q, const std::vector<double>& dq = {},
const std::vector<double>& 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<double>& q, const std::vector<double>& dq,
const std::vector<double>& kp, const std::vector<double>& kd);
/// 单关节 MIT 透传 (0x04)。⚠ 绕过规划; 须调用方维持 >=10Hz。
void send_mit(int idx, double q, double dq, double kp, double kd, double tau);
/// 全臂单帧透传 (0x05)。⚠ 同上。
Expand Down
8 changes: 8 additions & 0 deletions include/litearm/protocol.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
17 changes: 17 additions & 0 deletions src/arm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1233,6 +1233,23 @@ void Arm::move_js(const std::vector<double>& q, const std::vector<double>& dq,
cmd_expect_ack(proto::CMD_MOVE_JS, payload, "move_js");
}

void Arm::joint_follow(const std::vector<double>& q, const std::vector<double>& dq,
const std::vector<double>& kp, const std::vector<double>& kd) {
reject_if_in_dfu();
// 四条都排在本 arity 预检里, 顺序与载荷顺序一致 —— 报出来的名字就是缺的那个数组。
const std::vector<std::pair<const std::vector<double>*, 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<double> 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 越界");
Expand Down
20 changes: 8 additions & 12 deletions src/protocol.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,12 @@ const char* const kFlagNames[] = {"FAULT", "WD_TRIPPED", "FB_STALE",
// 命令覆盖契约 —— 与 Python 侧 _protocol.COMMAND_COVERAGE 逐条一致。
const std::vector<CommandCoverage> 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"},
Expand Down Expand Up @@ -288,17 +288,13 @@ const std::map<uint8_t, std::string>& firmware_only_rsps() {
const std::map<uint8_t, std::string>& 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<uint8_t, std::string> kGaps = {
{0x08, "CMD_JOINT_FOLLOW"},
};
// ⚠ 别顺手往这里塞东西: 空**是**"当前没有已知缺口"这句话的断言载体, 由
// `tests/test_protocol.cpp` 钉住。真出现新缺口时加条目**等于宣布**它, 加之前先想清楚。
static const std::map<uint8_t, std::string> kGaps;
return kGaps;
}

Expand Down
35 changes: 35 additions & 0 deletions tests/test_commands.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> q(7, 0.1);
const std::vector<double> dq(7, 0.2);
const std::vector<double> kp(7, 30.0);
const std::vector<double> kd(7, 1.5);
CHECK_NOTHROW(off->joint_follow(q, dq, kp, kd));

for (int missing = 0; missing < 4; ++missing) {
std::vector<double> args[4] = {q, dq, kp, kd};
args[missing] = std::vector<double>(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));
Expand Down
3 changes: 3 additions & 0 deletions tests/test_full_coverage.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>(7, 50.0),
std::vector<double>(7, 2.0), zeros7()); // 0x05
arm.joint_follow(zeros7(), zeros7(), std::vector<double>(7, 30.0),
std::vector<double>(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
Expand Down Expand Up @@ -370,6 +372,7 @@ static const std::set<std::string> 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",
Expand Down
20 changes: 10 additions & 10 deletions tests/test_protocol.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t> 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<uint8_t> seen;
for (const auto& c : proto::command_coverage()) {
Expand All @@ -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` 双向对表, 而这里没有固件树。所以**别把这条读成"已经跟固件对过账"**
Expand Down
Loading