Publish standalone A7 direct CAN SDK with examples and offline tests

This commit is contained in:
2026-10-27 13:57:39 +08:00
commit 685c4d240e
28 changed files with 2233 additions and 0 deletions
+8
View File
@@ -0,0 +1,8 @@
.venv/
__pycache__/
*.py[cod]
*.egg-info/
build/
dist/
out/
config/local*.json
+49
View File
@@ -0,0 +1,49 @@
# A7 机械臂直连 CAN SDK
通过 Linux SocketCAN 直接访问 A7 机械臂电机的 Python SDK。收集自已使用的 a7-left-can-control 项目,包含本地协议实现、读写封装、反馈保护、假 CAN 和手册协议编码器。
这是本地维护的直连 CAN 实现,不是厂家 LBot 1.0.5 TCP 在线控制器动态库。此仓库交付 SDK;上层 ServoPy/Pinocchio/Ruckig 轨迹规划和杯子抓取流程不属于 SDK 依赖。
## 安装
Linux、系统 Python 3.12 + venv,真机传输需要 SocketCAN 和 iproute2。SDK 运行时仅使用 Python 标准库,不依赖 ROS、python-can 或 C++ 编译。
~~~bash
bash scripts/setup.sh
bash scripts/check.sh
~~~
check 只使用内存假 CAN、报文编码和只读入口的计划模式,不连接真机。
## 示例
~~~bash
# 内存假电机
.venv/bin/python examples/fake_can.py
# 只显示读取计划,不连接 CAN
.venv/bin/python examples/read_angles.py
# 明确只读现场查询:核对 UID 后读取七轴位置
.venv/bin/python examples/read_angles.py --config config/left.json --samples 10 --execute
# 完整只读参数诊断;此命令会立即打开 CAN 并发送查询
.venv/bin/a7-can-inspect --config config/left.json --output out
~~~
config/left.json 是原设备的配置快照:ID61–67、host253、1 Mbps、前五轴 P80、后两轴 PA43。UID、方向、限位与 verified 字段仅适用于原设备与原现场,不代表新设备已完成校验。需由现场预先接线并配置 can0,SDK 不会自行配置 CAN 接口。
## 组成
| 模块 | 用途 |
| --- | --- |
| protocol.py | 已使用的 29 位扩展 CAN 协议、参数及反馈解码 |
| client.py | MotorBus:身份、读取、受限写入、使能与停止确认 |
| transport.py | SocketCAN、独占锁、classic/FD 与帧类型检查 |
| configuration.py | 七轴映射与配置校验 |
| runtime.py / startup.py | 上层轨迹适配、显式启动状态机 |
| manual_protocol.py / manual_client.py | P80/P50、PA43 手册标准帧编码与受限客户端 |
| fake.py / trace.py | 内存假电机与执行日志 |
接口和控制边界见 [SDK 接口](docs/API.md),两套协议的区别与现场结果见 [协议说明](docs/PROTOCOLS.md)。
MotorBus 默认 allow_control=False;导入 SDK 不打开设备。实际位置写入仍要求显式开启控制并通过原身份、模式、反馈、限位等检查。模式5位置控制是现有使用路径;未核验的力矩/阻抗接口拒绝执行。停止/关闭的行为应按具体接口理解,不能套用 TCP 控制器“退出保持使能”的策略。
打包只调整 inspect_bus 的配置和输出路径参数,核心协议、控制参数与保护逻辑保持原样。验证范围见 [交付验证](docs/VALIDATION.md)。
+211
View File
@@ -0,0 +1,211 @@
{
"protocol": "a7_extended_v1",
"interface": "can0",
"bitrate": 1000000,
"verified": {
"zero_sign_verified": true,
"watchdog_stop_verified": true,
"static_scene_verified": true
},
"calibration_note": "User confirmed 2026-09-30: inherit previous A7 MoveIt zero and directions. Previous bridge passes joint radians without sign reversal or offset; URDF axes unchanged. No new physical calibration performed.",
"max_velocity_rad_s": 0.20943951023931956,
"max_acceleration_rad_s2": 0.3490658503988659,
"recording_speed_smoothing": true,
"motors": [
{
"joint": "L1_JOINT",
"motor_id": 61,
"host_id": 253,
"uid": "9d6b3b4e20713017",
"family": "P80",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-2.099980115890503,
3.699923515319824
]
},
{
"joint": "L2_JOINT",
"motor_id": 62,
"host_id": 253,
"uid": "65a430209c23370f",
"family": "P80",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-2.9670000076293945,
0.14800000190734863
]
},
{
"joint": "L3_JOINT",
"motor_id": 63,
"host_id": 253,
"uid": "a4623b859c023019",
"family": "P80",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-2.200000047683716,
2.200000047683716
]
},
{
"joint": "L4_JOINT",
"motor_id": 64,
"host_id": 253,
"uid": "827c3b8a00743015",
"family": "P80",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-0.7850000262260437,
1.7000000476837158
]
},
{
"joint": "L5_JOINT",
"motor_id": 65,
"host_id": 253,
"uid": "81393020a0133503",
"family": "P80",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-2.9000000953674316,
2.9000000953674316
]
},
{
"joint": "L6_JOINT",
"motor_id": 66,
"host_id": 253,
"uid": "bc7c3020a0133503",
"family": "PA43",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-1.5700000524520874,
1.5700000524520874
]
},
{
"joint": "L7_JOINT",
"motor_id": 67,
"host_id": 253,
"uid": "ad793020a0133504",
"family": "PA43",
"position_range": [
-12.57,
12.57
],
"sign": 1,
"offset_rad": 0.0,
"calibration_evidence": "User confirmed reuse of previous A7 MoveIt mapping on 2026-09-30; see mapping_source.",
"raw_limits": [
-1.5700000524520874,
1.5700000524520874
]
}
],
"static_positions": {
"R1_JOINT": -0.06529251288978477,
"R2_JOINT": 0.43421500122645074,
"R3_JOINT": -0.4275353627832459,
"R4_JOINT": -1.6084573614299802,
"R5_JOINT": 1.0363918143049995,
"R6_JOINT": 0.05389745937285362,
"R7_JOINT": 0.343524376287478,
"left_hand_thumb_cmc_roll": 0.0,
"left_hand_thumb_cmc_yaw": 0.0,
"left_hand_thumb_cmc_pitch": 0.0,
"left_hand_thumb_mcp": 0.0,
"left_hand_index_mcp_roll": 0.0,
"left_hand_index_mcp_pitch": 0.0,
"left_hand_index_pip": 0.0,
"left_hand_middle_mcp_roll": 0.0,
"left_hand_middle_mcp_pitch": 0.0,
"left_hand_middle_pip": 0.0,
"left_hand_ring_mcp_roll": 0.0,
"left_hand_ring_mcp_pitch": 0.0,
"left_hand_ring_pip": 0.0,
"left_hand_pinky_mcp_roll": 0.0,
"left_hand_pinky_mcp_pitch": 0.0,
"left_hand_pinky_pip": 0.0
},
"mapping_source": {
"kind": "inherited_user_confirmed",
"date": "2026-09-30",
"sources": [
"ros2/nodes/observed_joint_states.py",
"ros2/nodes/lbot_trajectory_bridge.py",
"config/can_joint_mapping_current.json"
],
"formula": "q_urdf = q_motor; same joint zero/direction confirmed by user",
"angle_representation": "Preserve current encoder branch within configured mechanical bounds; do not clamp feedback or force pi wrapping."
},
"verification_evidence": {
"watchdog_stop_verified": {
"source": "user_confirmation",
"confirmed_at": "2026-09-30T16:13:49",
"statement": "可以,这个已经核实了",
"context": "User confirmed the two commissioning items after explanation of CAN-loss stop verification and hybrid right-online/left-CAN scene verification.",
"agent_retested_hardware": false
},
"static_scene_verified": {
"source": "user_confirmation",
"confirmed_at": "2026-09-30T16:13:49",
"statement": "可以,这个已经核实了",
"context": "User confirmed the two commissioning items after explanation of CAN-loss stop verification and hybrid right-online/left-CAN scene verification.",
"agent_retested_hardware": false
}
},
"apply_configured_speed_limit": true,
"speed_limit_confirmation": {
"date": "2026-10-10",
"source": "User requested continue commissioned position replay with a modest speed increase; 10 to 12 deg/s",
"hardware_maximum_verified": false,
"planning_and_drive_speed_deg_s": 12,
"measured_speed_guard_deg_s": 15
},
"servo_hz": 500,
"can_tx_hz": 250,
"max_jerk_rad_s3": 1.7453292519943295,
"tracking_error_rad": 0.16,
"feedback_max_age_s": 0.1,
"hardware_frequency_validated": false,
"experimental_servo_hz": 1000,
"measured_velocity_limit_rad_s": 0.2617993877991494,
"recording_time_parameterization": "moveit_totg"
}
+36
View File
@@ -0,0 +1,36 @@
# SDK 接口
## 扩展 CAN
~~~python
from a7_can_sdk import MotorBus
from a7_can_sdk.transport import SocketCan
from a7_can_sdk.configuration import load
cfg = load('config/left.json')
# 构造 SocketCan 会打开接口;以下示例只发送读取请求。
bus = MotorBus(SocketCan(cfg['interface'], cfg['bitrate']), cfg['motors'], allow_control=False)
try:
bus.start()
for motor in cfg['motors']:
mid = motor['motor_id']
uid = bus.identify(mid)
position_rad = bus.read_parameter(mid, 'position')
finally:
bus.close()
~~~
主要接口:
- identify(id):查询并核对配置中的 UID。
- read_parameter(id, name):按参数类型读回,未支持或超时明确报错。
- get_state(id):返回已有反馈及时间戳;未知物理速度/力矩保留 None。
- enable(...):仅 allow_control=True 时可调用,先执行原静止/身份/模式/目标预置等核对,再使能。
- set_positions({id: radians}):模式5位置目标,完整检查后再发送。
- disable():请求停止/掉使能,返回确认结果;没有确认不得当成已停。
- close():结束线程并关闭传输;控制会话会按原拥有的电机及停止规则处理,不能把它当成保持使能命令。
- set_torque / set_impedance:未完成 commissioning,抛出 NotCommissionedError。
Runtime 是与 ROS 无关的适配层,接受上层提供的 spline 和关节边界;它不是 MoveJ 控制器,也不是完整避障规划器。StartupEnable 只有显式 requested=True 且模式合适才会进入一次使能流程,不会故障重试。
反馈时间是主机接收时间。配置限位/应用速度上限不等同独立测得的机械硬限位或额定能力。SocketCan 的独占文件锁只能协调遵守同一锁的程序,不能阻止其他主机或不使用该锁的程序竞争总线。
+19
View File
@@ -0,0 +1,19 @@
# 协议与验证范围
生产扩展协议为 a7_extended_v1:29 位扩展 classic CAN,左臂 ID61–67、host253、1 Mbps,现有路径使用模式5位置目标。
P80Codec / PA43Codec 根据本地 260708 版本手册实现标准帧编码,使用独立模式枚举。CommunicationRanges 必须由实际通信量程提供,未指定时不猜物理速度/力矩。编码器只返回 Frame,不发送。ManualClient 默认为只读;受限写入还要求 allow_control=True、protocol_verified=True,模式切换须确认已掉使能。该声明不能替代现场验证。
| 项目 | P80/P50 | PA43 |
| --- | --- | --- |
| 参数查询 | 标准 ID=电机 ID,Byte0 命令 | 标准 ID=0x600+电机 ID |
| 参数标量 | 未说明的字节序须显式给定 | 32 位 little endian |
| MIT 单轴 | 位置16位,速度/Kp/Kd/力矩各10位 | 位置16位,其余各12位 |
| MIT 广播 | CAN FD 标准 ID0,8槽,位置16位其余12位 | CAN FD 标准 ID0x20,8槽 |
| 使能/停止/状态 | CMD 0x2A / 0x2B | 前7字节FF,末字节FC/FD/FA |
广播要求符合 ID1–8 的完整槽序,不把 ID61–67 塞进广播槽。CAN FD 需预先配置并验证 MTU、数据波特率;SDK 不会自动切换设备协议。
2026-10-09 历史现场只读结果:七轴扩展 UID/模式共14次成功,模式均5;七轴标准 classic 版本/模式/状态查询共21次没有标准帧响应。不能把手册编码测试通过说成当前 A7 已支持标准模式或 CAN FD 控制。力矩量程、增益及标准协议控制仍需独立核验。
没有移植 OTA、Flash 保存、机械设零或电角度校准命令。本次上传不执行现场查询或运动。
+12
View File
@@ -0,0 +1,12 @@
# 交付验证(2026-10-27)
- 新建系统 Python 3.12 venv,pip 安装独立 a7-can-sdk 成功;运行时无第三方 Python 依赖。
- 运行本 SDK 对应的两组测试,共26项全部通过:扩展帧固定向量、参数读取、只读限制、UID、使能前预置、故障/过期停止、停止确认,以及 P80/PA43 标准/FD 编解码。
- 内存假 CAN 示例成功读取 UID 和约0.1 rad位置;真实读取示例默认只打印计划,无硬件连接。
- 未运行上层轨迹项目的全量回归;未进行实机读取、使能、参数写入或运动。
源码来自 a7-left-can-control 的 src/a7_can_sdk。本次 SDK 核心文件保留原字节,只有 inspect_bus.py 将配置改为显式 --config、输出改为 --output(默认 out),使其可作为独立安装入口;未更改控制逻辑。
source_manifest.json 保存本次收集文件的原始哈希;inherited_source_manifest.json 是原项目更早的来源记录,不代表全部文件仍与早期版本相同。package_manifest.json 对应本次交付内容。
历史实机结果与协议限制见 PROTOCOLS.md。离线通过不证明任意电机固件或 CAN FD 的现场兼容性。
+15
View File
@@ -0,0 +1,15 @@
{
"ros2/a7_can_sdk/__init__.py": "e31fc05e571aa17a27a9b434996ac47e975e15095ca2f5a5693e08458cdd0bae",
"ros2/a7_can_sdk/client.py": "1214b83e09b69efdb7516fcdf56d03b5aa2b67fa3796a31f2d570ff7c90e6d50",
"ros2/a7_can_sdk/configuration.py": "a6bfdcf3f4586c1ac10a6895118fb1ffa7f0972d32e68b47920bec33491653f7",
"ros2/a7_can_sdk/fake.py": "f3303e48e2e602b639350a5f27aecc68e0995aeba462c2927c26d8eb0c7c9ef1",
"ros2/a7_can_sdk/inspect_bus.py": "c307d60bcc3ce348984f7ff76bb00f23df57fb12b9d63c8921b6a9abdce19cf0",
"ros2/a7_can_sdk/protocol.py": "018fa9b432b3c175cfbdab87c30c1f2d73ff3de46a855ba269f848a6f5912c4f",
"ros2/a7_can_sdk/runtime.py": "dbc0e900b77207323f93e6ee23644bc1b3804e88affe0043be5d89ca567bc759",
"ros2/a7_can_sdk/startup.py": "7bd4956948970cb66e79df9f100ea6b35ec6ab815e091b06a529eaa17fea29c6",
"ros2/a7_can_sdk/trace.py": "b5e49536ed37f102a9f463146d91ccd3796b92876d38f1cbbc3add4b88978082",
"ros2/a7_can_sdk/transport.py": "fe5cea42798f30a27b7d1ccc7427725f12e1577714c7fdb923d5c3d8513f20de",
"ros2/config/extended_can_left.json": "c8e73d71b7458524433976acc1560f52046ff0754d63420c81af0eda3111375e",
"ros2/generated/a7_l20.urdf": "a05c6e3cb62543f6dbcca2524a499fa36e18fe83b6ab55fb1c08bfc638ed7370",
"ros2/tests/test_a7_can_sdk.py": "c67236e3671989fe000a6d7252ec64fdba2e060cf161a07417c64d75d73cd6c1"
}
+139
View File
@@ -0,0 +1,139 @@
{
"files": [
{
"file": ".gitignore",
"bytes": 79,
"sha256": "6e4768f2370f7127fd89b0a840a20e01343a13d85e253fc5d0dad71302f19b03"
},
{
"file": "README.md",
"bytes": 2772,
"sha256": "263c2cd604cf36a3ab2a559c5b2fbfc17274deb1379f7d9355511222e82440d1"
},
{
"file": "config/left.json",
"bytes": 6521,
"sha256": "fc9811a619ed4ece633e303932f49f68cedd2ddc0eaca94e201f0b27ba75bab8"
},
{
"file": "docs/API.md",
"bytes": 1862,
"sha256": "70cdc5af0ffefa1f9703eeb2c6c5855d2354766bac445c15a9483f4d8acb19c7"
},
{
"file": "docs/PROTOCOLS.md",
"bytes": 1677,
"sha256": "588c852cd0557baaaf438256a13f9c098c844aae4e7d1cb0b8564007434ac351"
},
{
"file": "docs/VALIDATION.md",
"bytes": 1192,
"sha256": "796a031a1999479b91c20674d9e60e871d4bf2c9f2170bf433eb2e169dadeca9"
},
{
"file": "docs/inherited_source_manifest.json",
"bytes": 1322,
"sha256": "09c7664c108b580d87d805af0f9855fa1c980affec1c51ee4cb69e36cd57c3e1"
},
{
"file": "docs/source_manifest.json",
"bytes": 2175,
"sha256": "305e1784fa7746900732007f68018fb71f0b6bb6e5935839b4553b176109c2b0"
},
{
"file": "examples/fake_can.py",
"bytes": 633,
"sha256": "ac1b5200635133529a1b49e639c5cf83097126bff31957258029edf44e62c29a"
},
{
"file": "examples/read_angles.py",
"bytes": 1523,
"sha256": "24ce5d4d3a6770d7ec14d095a47fd99f96a981dd59180f15412c9e582a0d9f99"
},
{
"file": "pyproject.toml",
"bytes": 371,
"sha256": "940bd1c5beeb1ef43baaa03c40ce2b57b38d50dd7adec63cf095262cd9bc381b"
},
{
"file": "scripts/check.sh",
"bytes": 287,
"sha256": "d55e8ae6a40ff04c7e3696870464f9c3206500025fbdb04f143f7638bc574590"
},
{
"file": "scripts/setup.sh",
"bytes": 200,
"sha256": "057c1fe854f8987101803cc8fada07d1561544619374409765c8800a55953f49"
},
{
"file": "src/a7_can_sdk/__init__.py",
"bytes": 504,
"sha256": "82a67f0691d40ee464b2bcce758b5b48122be6d7472df5acd2688adffd87c743"
},
{
"file": "src/a7_can_sdk/client.py",
"bytes": 13256,
"sha256": "977798ec27540206c14ceed514f3ae546bc563053b1330cb24416f77b70c0828"
},
{
"file": "src/a7_can_sdk/configuration.py",
"bytes": 2344,
"sha256": "ca68035f4c328e74c8c6716c4c29fbc87ad81e5761431fd5ca086daf689bbb63"
},
{
"file": "src/a7_can_sdk/fake.py",
"bytes": 3429,
"sha256": "f3303e48e2e602b639350a5f27aecc68e0995aeba462c2927c26d8eb0c7c9ef1"
},
{
"file": "src/a7_can_sdk/inspect_bus.py",
"bytes": 1761,
"sha256": "9a0b69fb061ecc9ec992781282b99f51ac7c919749287559bfc8507c4df509b8"
},
{
"file": "src/a7_can_sdk/manual_client.py",
"bytes": 5508,
"sha256": "3aaa99d0a867dac2b64a161c7d3d8f2e9d80fd3ba92d98ba4a1bfe2914ba7357"
},
{
"file": "src/a7_can_sdk/manual_protocol.py",
"bytes": 16310,
"sha256": "9578c24d21b74b02a32132858d1bfeb4ff2c359a8faf131796c2d5d3c159d961"
},
{
"file": "src/a7_can_sdk/protocol.py",
"bytes": 8211,
"sha256": "6f67b940880d851027608b8671bf7124503252309b3cd1951ed1df8a9a47915a"
},
{
"file": "src/a7_can_sdk/runtime.py",
"bytes": 9223,
"sha256": "dbc0e900b77207323f93e6ee23644bc1b3804e88affe0043be5d89ca567bc759"
},
{
"file": "src/a7_can_sdk/startup.py",
"bytes": 1599,
"sha256": "7bd4956948970cb66e79df9f100ea6b35ec6ab815e091b06a529eaa17fea29c6"
},
{
"file": "src/a7_can_sdk/trace.py",
"bytes": 1599,
"sha256": "b5e49536ed37f102a9f463146d91ccd3796b92876d38f1cbbc3add4b88978082"
},
{
"file": "src/a7_can_sdk/transport.py",
"bytes": 3811,
"sha256": "fdd3cd9d1404ce9d0b9ea6e95725c2ea57e116c11cb526a4907cd3440a8ba220"
},
{
"file": "tests/test_manual_protocol.py",
"bytes": 9966,
"sha256": "c021ade68c5556d7a7de4e5115ffc8b7dc03df675e2aa56d84aad1c370ec4948"
},
{
"file": "tests/test_sdk_inherited.py",
"bytes": 6009,
"sha256": "eac3f824ad5119abe2be507fee86841c6411dd2be7569eb5a84487d30a9fb70d"
}
]
}
+62
View File
@@ -0,0 +1,62 @@
{
"source_project": "a7-left-can-control",
"source_component": "src/a7_can_sdk",
"files": [
{
"file": "src/a7_can_sdk/__init__.py",
"source_sha256": "82a67f0691d40ee464b2bcce758b5b48122be6d7472df5acd2688adffd87c743"
},
{
"file": "src/a7_can_sdk/client.py",
"source_sha256": "977798ec27540206c14ceed514f3ae546bc563053b1330cb24416f77b70c0828"
},
{
"file": "src/a7_can_sdk/configuration.py",
"source_sha256": "ca68035f4c328e74c8c6716c4c29fbc87ad81e5761431fd5ca086daf689bbb63"
},
{
"file": "src/a7_can_sdk/fake.py",
"source_sha256": "f3303e48e2e602b639350a5f27aecc68e0995aeba462c2927c26d8eb0c7c9ef1"
},
{
"file": "src/a7_can_sdk/inspect_bus.py",
"source_sha256": "c307d60bcc3ce348984f7ff76bb00f23df57fb12b9d63c8921b6a9abdce19cf0"
},
{
"file": "src/a7_can_sdk/manual_client.py",
"source_sha256": "3aaa99d0a867dac2b64a161c7d3d8f2e9d80fd3ba92d98ba4a1bfe2914ba7357"
},
{
"file": "src/a7_can_sdk/manual_protocol.py",
"source_sha256": "9578c24d21b74b02a32132858d1bfeb4ff2c359a8faf131796c2d5d3c159d961"
},
{
"file": "src/a7_can_sdk/protocol.py",
"source_sha256": "6f67b940880d851027608b8671bf7124503252309b3cd1951ed1df8a9a47915a"
},
{
"file": "src/a7_can_sdk/runtime.py",
"source_sha256": "dbc0e900b77207323f93e6ee23644bc1b3804e88affe0043be5d89ca567bc759"
},
{
"file": "src/a7_can_sdk/startup.py",
"source_sha256": "7bd4956948970cb66e79df9f100ea6b35ec6ab815e091b06a529eaa17fea29c6"
},
{
"file": "src/a7_can_sdk/trace.py",
"source_sha256": "b5e49536ed37f102a9f463146d91ccd3796b92876d38f1cbbc3add4b88978082"
},
{
"file": "src/a7_can_sdk/transport.py",
"source_sha256": "fdd3cd9d1404ce9d0b9ea6e95725c2ea57e116c11cb526a4907cd3440a8ba220"
},
{
"file": "tests/test_manual_protocol.py",
"source_sha256": "c021ade68c5556d7a7de4e5115ffc8b7dc03df675e2aa56d84aad1c370ec4948"
},
{
"file": "tests/test_sdk_inherited.py",
"source_sha256": "eac3f824ad5119abe2be507fee86841c6411dd2be7569eb5a84487d30a9fb70d"
}
]
}
+16
View File
@@ -0,0 +1,16 @@
"""Read an in-memory motor; no SocketCAN or hardware connection."""
from a7_can_sdk import MotorBus
from a7_can_sdk.fake import FakeCan
from a7_can_sdk.protocol import PARAMETERS, read_parameter
motors = [dict(motor_id=61, host_id=253, uid='000000000000003d', raw_limits=[-2., 2.])]
frame = read_parameter(61, PARAMETERS['position'])
print('Position query:', hex(frame.can_id), frame.data.hex())
peer = FakeCan(motors, [.1])
bus = MotorBus(peer, motors, allow_control=False)
try:
bus.start()
print('Fake UID:', bus.identify(61))
print('Fake position (rad):', bus.read_parameter(61, 'position'))
finally:
bus.close()
+37
View File
@@ -0,0 +1,37 @@
"""Explicit read-only position queries. Default invocation does not open CAN."""
import argparse
import json
import time
from pathlib import Path
from a7_can_sdk import MotorBus
from a7_can_sdk.configuration import load
from a7_can_sdk.transport import SocketCan
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--config', type=Path)
parser.add_argument('--execute', action='store_true', help='Open CAN and send read-only queries')
parser.add_argument('--samples', type=int, default=10)
args = parser.parse_args()
if not args.execute:
print('Plan only: identify configured motors and query positions; no hardware connection.')
return
if args.config is None or args.samples < 1:
parser.error('--execute requires --config and --samples >= 1')
cfg = load(args.config)
bus = MotorBus(SocketCan(cfg['interface'], cfg['bitrate']), cfg['motors'], allow_control=False)
try:
bus.start()
for motor in cfg['motors']:
if bus.identify(motor['motor_id']) != motor['uid']:
raise RuntimeError('Motor identity mismatch')
for _ in range(args.samples):
rows = [{'motor_id': m['motor_id'], 'position_rad': bus.read_parameter(m['motor_id'], 'position')}
for m in cfg['motors']]
print(json.dumps({'host_time_ns': time.time_ns(), 'joints': rows}))
time.sleep(.1)
finally:
bus.close()
if __name__ == '__main__':
main()
+16
View File
@@ -0,0 +1,16 @@
[build-system]
requires = ["setuptools>=68"]
build-backend = "setuptools.build_meta"
[project]
name = "a7-can-sdk"
version = "0.1.0"
description = "Direct SocketCAN SDK for commissioned A7 arm motor protocols"
requires-python = ">=3.12"
dependencies = []
[project.scripts]
a7-can-inspect = "a7_can_sdk.inspect_bus:main"
[tool.setuptools.packages.find]
where = ["src"]
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env bash
set -euo pipefail
SDK_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
cd -- "$SDK_ROOT"
"$SDK_ROOT/.venv/bin/python" -m unittest discover -s tests -v
"$SDK_ROOT/.venv/bin/python" examples/fake_can.py
"$SDK_ROOT/.venv/bin/python" examples/read_angles.py
+5
View File
@@ -0,0 +1,5 @@
#!/usr/bin/env bash
set -euo pipefail
SDK_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
python3.12 -m venv "$SDK_ROOT/.venv"
"$SDK_ROOT/.venv/bin/python" -m pip install -e "$SDK_ROOT"
+8
View File
@@ -0,0 +1,8 @@
"""A7 extended CAN motor SDK. Importing never opens hardware."""
from .protocol import Frame, MotorState, Parameter, PARAMETERS
from .client import MotorBus, NotCommissionedError
from .manual_protocol import P80Codec, PA43Codec, P80Mode, PA43Mode, CommunicationRanges
from .manual_client import ManualClient
__all__ = ['Frame', 'MotorState', 'Parameter', 'PARAMETERS', 'MotorBus', 'NotCommissionedError',
'P80Codec', 'PA43Codec', 'P80Mode', 'PA43Mode', 'CommunicationRanges', 'ManualClient']
+249
View File
@@ -0,0 +1,249 @@
"""Threaded motor API: one receive owner, serialized queries, independent guard."""
from dataclasses import replace
import math
import struct
import threading
import time
from .protocol import PARAMETERS, command, decode_parameter, decode_state, read_parameter, write_parameter
class NotCommissionedError(RuntimeError):pass
class MotorBus:
def __init__(self, transport, motors, allow_control=False, stale_after=.25):
self.transport=transport;self.motors={m['motor_id']:m for m in motors}
if len(self.motors)!=len(motors):raise ValueError('Duplicate motor IDs')
self.allow_control=allow_control;self.stale_after=stale_after
self._cv=threading.Condition(threading.RLock());self._query=threading.Lock()
self._tx=threading.RLock();self._operation=threading.RLock();self._quit=threading.Event()
self._states={};self._parameters={};self._responses={};self._uids={};self._fault_reports={}
self._owned=set();self._pending_stops=set();self.stop_acknowledged=None;self._enable_grace=0.;self._threads=[];self.fault=''
self.effective_speed_limits={}
self.stats=dict(rx=0,tx=0,query_timeouts=0,unsupported_parameters=0, max_send_ms=0., max_position_batch_ms=0.)
def start(self):
if self._threads:raise RuntimeError('Already started')
self._threads=[threading.Thread(target=f,daemon=True,name=n) for f,n in
[(self._rx,'a7_can_rx'),(self._guard,'a7_can_guard')]]
for t in self._threads:t.start()
return self
def _send(self, frame):
with self._tx:
if self._quit.is_set():raise RuntimeError('SDK closed')
started=time.monotonic()
try:self.transport.send(frame)
finally:self.stats['max_send_ms']=max(self.stats['max_send_ms'],(time.monotonic()-started)*1000)
self.stats['tx']+=1
def _device(self, mid):
if mid not in self.motors:raise ValueError('Motor not configured')
return self.motors[mid]
def _rx(self):
while not self._quit.is_set():
try:
f=self.transport.receive(.02)
if f is None:continue
if not f.extended or f.is_fd:continue
now=time.monotonic();kind=f.can_id>>24;mid=(f.can_id>>8)&255;host=f.can_id&255
if mid not in self.motors:continue
with self._cv:
self.stats['rx']+=1
if kind==0 and host==254:self._uids[mid]=(now,f.data.hex())
elif host==self.motors[mid].get('host_id',253):
if kind in (2,24):
self._states[mid]=decode_state(f,now,tuple(self.motors[mid].get('position_range',[-12.57,12.57])))
elif kind==17:
_,_,index,status,value=decode_parameter(f)
self._responses[(mid,index)]=(now,status,value)
if status==0:self._parameters[(mid,index)]=(now,value)
elif kind==21:self._fault_reports[mid]=struct.unpack('<II',f.data)
self._cv.notify_all()
except Exception as exc:
with self._cv:self.fault=f'CAN receive: {exc}';self._cv.notify_all()
self._quit.wait(.02)
def _wait(self, collection, key, since, timeout):
end=time.monotonic()+timeout
with self._cv:
while not self._quit.is_set():
item=collection.get(key)
if item and item[0]>=since:return item
if self.fault:raise OSError(self.fault)
remaining=end-time.monotonic()
if remaining<=0:
self.stats['query_timeouts']+=1;raise TimeoutError(f'No response: {key}')
self._cv.wait(remaining)
raise RuntimeError('SDK closed')
def identify(self, mid, timeout=.25):
cfg=self._device(mid)
with self._query:
stamp=time.monotonic();self._send(command(0,mid,cfg.get('host_id',253)))
_,uid=self._wait(self._uids,mid,stamp,timeout)
if not cfg.get('uid') or uid!=cfg['uid']:raise ValueError(f'ID{mid} UID mismatch')
return uid
def read_parameter(self, mid, name, timeout=.20):
cfg=self._device(mid);p=PARAMETERS[name]
with self._query:
stamp=time.monotonic();self._send(read_parameter(mid,p,cfg.get('host_id',253)))
_,status,value=self._wait(self._responses,(mid,p.index),stamp,timeout)
if status or value is None:
self.stats['unsupported_parameters']+=1
raise NotImplementedError(f'ID{mid}: {name} unavailable, status={status}')
return value
def write_parameter(self, mid, name, value):
if not self.allow_control:raise PermissionError('Read-only SDK')
cfg=self._device(mid);p=PARAMETERS[name]
if name=='speed_limit' and not 0<value<=math.radians(20):raise ValueError('SDK application speed ceiling 0..20 degrees/s; not hardware rating')
if name=='position_target':
lo,hi=cfg.get('raw_limits',cfg.get('position_range',[-12.57,12.57]))
if not lo<=value<=hi:raise ValueError('Position outside configured raw limits')
self._send(write_parameter(mid,p,value,cfg.get('host_id',253)))
actual=self.read_parameter(mid,name)
tol=2*25.14/65535 if name=='position_target' else 1e-5
if not math.isclose(actual,value,abs_tol=tol,rel_tol=1e-5):raise ValueError(f'{name} readback mismatch')
return actual
def get_state(self, mid, max_age=None):
self._device(mid);limit=self.stale_after if max_age is None else max_age
with self._cv:
s=self._states.get(mid);now=time.monotonic()
if s is None or now-s.timestamp>limit:raise ValueError(f'ID{mid} missing/stale feedback')
optional={}
for key,name in [('velocity_rad_s','velocity'),('torque_nm','torque'),('current_a','current'),('bus_voltage_v','bus_voltage')]:
t,v=self._parameters.get((mid,PARAMETERS[name].index),(0,None))
optional[key]=v if now-t<.6 else None
faults,warnings=self._fault_reports.get(mid,(0,0))
return replace(s,**optional,fault_report=faults,warning_report=warnings)
def get_states(self):return {mid:self.get_state(mid) for mid in self.motors}
def diagnostics(self):
rows={}
for mid in self.motors:
try:rows[mid]=self.get_state(mid).to_dict()
except ValueError as exc:rows[mid]={'error':str(exc)}
with self._cv:
parameters={mid:{name:dict(index=hex(p.index), unit=p.unit, value=self._parameters[(mid,p.index)][1],
age_s=time.monotonic()-self._parameters[(mid,p.index)][0])
for name,p in PARAMETERS.items() if (mid,p.index) in self._parameters}
for mid in self.motors}
identities={mid:item[1] for mid,item in self._uids.items()}
return dict(motors=rows,parameters=parameters,identities=identities,
stop_acknowledged=self.stop_acknowledged,stats=dict(self.stats),fault=self.fault,
effective_speed_limits_rad_s=dict(self.effective_speed_limits),
allow_control=self.allow_control,force_control_available=False)
def _healthy(self, enabled=None):
if self.fault:raise ValueError(self.fault)
rows=self.get_states()
for mid,s in rows.items():
if s.fault_bits or s.fault_report:raise ValueError(f'ID{mid} motor fault')
if enabled is not None and s.enabled!=(mid in enabled):raise ValueError(f'ID{mid} enable state unexpected')
if s.raw_position in (0,65535):raise ValueError('Position feedback saturated')
return rows
def enable(self, motor_ids=None, speed_limit=math.radians(2), apply_configured_speed_limit=False):
if not self.allow_control:raise PermissionError('Read-only SDK')
ids=list(self.motors if motor_ids is None else motor_ids)
if len(ids)!=len(set(ids)) or not ids:raise ValueError('Unique motor IDs required')
for mid in ids:self._device(mid)
with self._operation:
self._healthy(enabled=set())
positions={}
for mid in ids:
self.identify(mid)
if self.read_parameter(mid,'control_mode')!=5:raise ValueError('Position mode 5 required; no automatic switch')
q=self.read_parameter(mid,'position')
if abs(q-self.get_state(mid).position_rad)>.03:raise ValueError('Position encoding not validated')
# Velocity telemetry is not an enable gate (user-requested policy).
old_speed=self.read_parameter(mid,'speed_limit')
if old_speed<=0:raise ValueError('Invalid hardware speed limit')
self.effective_speed_limits[mid]=self.write_parameter(mid,'speed_limit',speed_limit if apply_configured_speed_limit else min(old_speed,speed_limit))
self.write_parameter(mid,'position_target',q);positions[mid]=q
try:
for mid in ids:
if abs(self.read_parameter(mid,'position')-positions[mid])>.003:raise ValueError('Arm moved during enable preparation')
with self._cv:self._owned.add(mid);self._enable_grace=time.monotonic()+.3
self._send(command(3,mid,self.motors[mid].get('host_id',253)))
end=time.monotonic()+.3
while time.monotonic()<end:
try:self._healthy(enabled=set(ids));return positions
except ValueError:time.sleep(.005)
raise ValueError('Enable acknowledgement missing')
except BaseException:self.disable(ids);raise
def set_positions(self, positions):
started=time.monotonic()
if not self.allow_control:raise PermissionError('Read-only SDK')
with self._tx:
states=self._healthy(enabled=set(self._owned))
for mid,q in positions.items():
cfg=self._device(mid)
if mid not in self._owned or not states[mid].enabled:raise ValueError('Motor not enabled by SDK')
lo,hi=cfg.get('raw_limits',cfg.get('position_range',[-12.57,12.57]))
if not math.isfinite(q) or not lo<=q<=hi:raise ValueError('Invalid position command')
timings=[]
for mid,q in positions.items():
begin=time.monotonic()
self._send(write_parameter(mid,PARAMETERS['position_target'],q,self.motors[mid].get('host_id',253)))
timings.append(dict(motor_id=mid,send_start_s=begin,send_end_s=time.monotonic(),target_raw_rad=q))
self.stats['max_position_batch_ms']=max(self.stats['max_position_batch_ms'],(time.monotonic()-started)*1000)
return timings
def _send_stops(self, ids):
errors=[]
with self._tx:
with self._cv:self._owned.difference_update(ids);self._pending_stops.update(ids)
for mid in ids:
try:self._send(command(4,mid,self.motors[mid].get('host_id',253)))
except Exception as exc:errors.append(str(exc))
return errors
def disable(self, motor_ids=None):
if not self.allow_control:raise PermissionError('Read-only SDK')
ids=list(self.motors if motor_ids is None else motor_ids)
for mid in ids:self._device(mid)
self.stop_acknowledged=False
stamp=time.monotonic();errors=self._send_stops(ids)
end=stamp+.3
while time.monotonic()<end:
try:
states=[self.get_state(mid) for mid in ids]
if all(s.timestamp>stamp and s.mode_state==0 for s in states):
with self._cv:self._pending_stops.difference_update(ids)
self.stop_acknowledged=not errors
return self.stop_acknowledged
except ValueError:pass
time.sleep(.005)
return False
def _guard(self):
while not self._quit.wait(.02):
with self._cv:owned=set(self._owned);grace=self._enable_grace
if owned and time.monotonic()>grace:
try:self._healthy(enabled=owned)
except Exception as exc:
self.fault=f'SDK guard: {exc}'
if not self.disable(owned):self.fault+='; stop not acknowledged'
def set_torque(self, motor_id, torque_nm):
raise NotCommissionedError('Reserved: torque scales, torque mode and force safety not commissioned; no frame sent')
def set_impedance(self, motor_id, position_rad, velocity_rad_s, kp, kd, feedforward_nm=0.):
raise NotCommissionedError('Reserved: per-motor MIT ranges and gains not commissioned; no frame sent')
def close(self):
stopped=True
ids=self._owned | self._pending_stops
if ids:stopped=self.disable(list(ids))
self._quit.set()
with self._cv:self._cv.notify_all()
for t in self._threads:t.join(timeout=1.)
self.transport.close()
return stopped
+36
View File
@@ -0,0 +1,36 @@
"""Measured bus identity is separate from unverified motor-to-URDF calibration."""
import json
import math
from pathlib import Path
CHECKS=('zero_sign_verified','watchdog_stop_verified','static_scene_verified')
def load(path, hardware=False):
cfg=json.loads(Path(path).read_text())
if cfg.get('protocol')!='a7_extended_v1':raise ValueError('Requires observed extended CAN configuration')
motors=cfg['motors']
if [m['joint'] for m in motors]!=[f'L{i}_JOINT' for i in range(1,8)]:raise ValueError('Ordered L1..L7 required')
if [m['motor_id'] for m in motors]!=list(range(61,68)):raise ValueError('Commissioned left IDs are 61..67')
if [m['family'] for m in motors]!=['P80']*5+['PA43']*2:raise ValueError('P80 x5 and PA43 x2 required')
for m in motors:
if len(bytes.fromhex(m['uid']))!=8:raise ValueError('Eight-byte UID required')
if m['sign'] not in (-1,1) or not math.isfinite(m['offset_rad']):raise ValueError('Invalid calibration')
if m.get('host_id')!=253:raise ValueError('Observed host ID is 253')
if cfg.get('bitrate')!=1000000:raise ValueError('Observed bitrate is 1 Mbps')
for value in cfg['static_positions'].values():
if not math.isfinite(value):raise ValueError('Nonfinite static position')
speed=cfg.get('max_velocity_rad_s',0);acc=cfg.get('max_acceleration_rad_s2',0)
if not 0<speed<=math.radians(20) or not 0<acc<=math.radians(40):raise ValueError('Application speed/acceleration ceiling: 20 deg/s, 40 deg/s2; not hardware ratings')
measured=cfg.get('measured_velocity_limit_rad_s',speed)
if not math.isfinite(measured) or not speed<=measured<=math.radians(30):
raise ValueError('Measured velocity guard must be between planned speed and 30 deg/s')
if not 0<cfg.get('tracking_error_rad',.08)<=.2 or not 0<cfg.get('feedback_max_age_s',.05)<=.1:
raise ValueError('Tracking/feedback application ceilings exceeded')
if hardware:
missing=[name for name in CHECKS if cfg.get('verified',{}).get(name) is not True]
if missing:raise ValueError('Hardware execution not commissioned: '+', '.join(missing))
for m in motors:
if not m.get('calibration_evidence') or not m.get('raw_limits'):
raise ValueError('Per-motor calibration evidence and raw limits required')
return cfg
+65
View File
@@ -0,0 +1,65 @@
"""In-memory protocol peer; never opens SocketCAN. Used by tests and RViz demo."""
import math
import queue
import struct
import threading
import time
from .protocol import Frame, BY_INDEX, PARAMETERS
class FakeCan:
def __init__(self, motors, positions):
self.motors={m['motor_id']:m for m in motors}
self.positions=dict(zip(self.motors,positions));self.targets=dict(self.positions)
self.enabled=set();self.frames=[];self.drop_feedback=set();self.fault_bits={}
self.unsupported=set();self.ignore_enable=set();self.ignore_stop=set()
self.values={mid:{p.index:0. for p in PARAMETERS.values()} for mid in self.motors}
for mid,values in self.values.items():
values.update({0x7005:5,0x7017:math.radians(2),0x701c:48.,0x7018:10.,0x700b:10.,0x7028:0})
self.q=queue.Queue();self.lock=threading.RLock();self.quit=threading.Event()
self.thread=threading.Thread(target=self._loop,daemon=True);self.thread.start()
def send(self, frame):
with self.lock:
self.frames.append(frame)
mid=frame.can_id&255;host=(frame.can_id>>8)&255;kind=frame.can_id>>24
if mid not in self.motors:return
if kind==0:
self.q.put(Frame((mid<<8)|254,bytes.fromhex(self.motors[mid]['uid'])))
elif kind==3 and mid not in self.ignore_enable:self.enabled.add(mid)
elif kind==4 and mid not in self.ignore_stop:self.enabled.discard(mid)
elif kind in (17,18):
index=struct.unpack('<H',frame.data[:2])[0];p=BY_INDEX.get(index)
if kind==18 and p:
value=struct.unpack('<'+p.fmt,frame.data[4:4+struct.calcsize(p.fmt)])[0]
self.values[mid][index]=value
if index==0x7016:self.targets[mid]=value
if kind==17:
bad=p is None or (mid,index) in self.unsupported
value=self.positions[mid] if index==0x7019 else self.values[mid].get(index,0)
data=struct.pack('<H2x',index)+(bytes(4) if bad else struct.pack('<'+p.fmt,value).ljust(4,b'\0'))
self.q.put(Frame((17<<24)|(int(bad)<<16)|(mid<<8)|host,data))
def _loop(self):
previous=time.monotonic()
while not self.quit.wait(.01):
now=time.monotonic();dt=now-previous;previous=now
with self.lock:
for mid,cfg in self.motors.items():
old=self.positions[mid]
if mid in self.enabled:
step=self.values[mid][0x7017]*dt
self.positions[mid]+=max(-step,min(step,self.targets[mid]-old))
self.values[mid][0x701b]=(self.positions[mid]-old)/dt
if mid in self.drop_feedback:continue
lo,hi=cfg.get('position_range',[-12.57,12.57])
raw=max(0,min(65535,round((self.positions[mid]-lo)/(hi-lo)*65535)))
mode=2 if mid in self.enabled else 0
self.q.put(Frame((2<<24)|(mode<<22)|(self.fault_bits.get(mid,0)<<16)|(mid<<8)|cfg.get('host_id',253),
struct.pack('>4H',raw,32768,32768,250)))
def receive(self,timeout=.02):
try:return self.q.get(timeout=timeout)
except queue.Empty:return None
def close(self):self.quit.set();self.thread.join(timeout=1.)
+38
View File
@@ -0,0 +1,38 @@
"""Read-only identity, status and parameter capture. No enable, stop or writes."""
import argparse
from datetime import datetime
import json
from pathlib import Path
import time
from .client import MotorBus
from .configuration import load
from .protocol import PARAMETERS
from .transport import SocketCan
def main():
p=argparse.ArgumentParser(description=__doc__)
p.add_argument('--config', type=Path, required=True)
p.add_argument('--output', type=Path, default=Path('out'))
args=p.parse_args();cfg=load(args.config)
bus=MotorBus(SocketCan(cfg['interface'],cfg['bitrate']),cfg['motors']).start()
report={'read_only':True,'protocol':cfg['protocol'],'motors':{}}
try:
time.sleep(.15)
for m in cfg['motors']:
mid=m['motor_id'];row={'uid':bus.identify(mid),'parameters':{}}
for name,param in PARAMETERS.items():
try:row['parameters'][name]={'value':bus.read_parameter(mid,name),'unit':param.unit}
except (TimeoutError,NotImplementedError) as exc:row['parameters'][name]={'error':str(exc)}
row['state']=bus.get_state(mid).to_dict();report['motors'][mid]=row
report['diagnostics']=bus.diagnostics()
finally:bus.close()
out=args.output/('can_sdk_inspect_'+datetime.now().strftime('%Y%m%d_%H%M%S_%f'))
out.mkdir(parents=True);(out/'result.json').write_text(json.dumps(report,indent=2)+'\n')
for mid,row in report['motors'].items():
s=row['state'];errors=[k for k,v in row['parameters'].items() if 'error' in v]
print(f"ID{mid}: enabled={s['enabled']} mode={s['mode_state']} faults={s['faults']} temperature={s['temperature_c']}C unavailable={errors}")
print(out/'result.json')
if __name__=='__main__':main()
+102
View File
@@ -0,0 +1,102 @@
"""Synchronous manual-protocol client. One receive owner; read-only by default.
Do not share a transport with MotorBus. Standard protocol verification is
separate from the existing extended-protocol commissioning.
"""
import threading
import time
from .manual_protocol import P80Codec, PA43Codec
class ManualClient:
def __init__(self,transport,codec,*,allow_control=False,protocol_verified=False,timeout=.25,
can_fd=False,bitrate_switch=False):
if not isinstance(codec,(P80Codec,PA43Codec)):raise ValueError('Manual codec required')
if type(allow_control) is not bool or type(protocol_verified) is not bool:
raise ValueError('Explicit boolean control/verification declarations required')
if not 0<timeout<=2:raise ValueError('Bounded timeout required')
if type(can_fd) is not bool or type(bitrate_switch) is not bool or (bitrate_switch and not can_fd):
raise ValueError('Explicit CAN FD/BRS selection required')
self.transport=transport;self.codec=codec;self.timeout=timeout
self.allow_control=allow_control;self.protocol_verified=protocol_verified
self.can_fd=can_fd;self.bitrate_switch=bitrate_switch
self.lock=threading.RLock();self.trace=[];self.closed=False
def _log(self,direction,frame):
self.trace.append(dict(direction=direction,t_monotonic_s=time.monotonic(),can_id=frame.can_id,
extended=frame.extended,is_fd=frame.is_fd,fd_flags=frame.fd_flags,data_hex=frame.data.hex()))
def _query(self,request,decode):
with self.lock:
if self.closed:raise RuntimeError('Manual client closed')
if self.can_fd:request=request.as_fd(bitrate_switch=self.bitrate_switch)
for _ in range(200):
old=self.transport.receive(0.)
if old is None:break
self._log('rx_before_query',old)
self.transport.send(request);self._log('tx',request)
end=time.monotonic()+self.timeout
while True:
remaining=end-time.monotonic()
if remaining<=0:raise TimeoutError(f'{self.codec.family} ID{self.codec.motor_id}: no standard-protocol response')
frame=self.transport.receive(remaining)
if frame is None:continue
self._log('rx',frame)
value=decode(frame)
if value is not None:return value
def read_parameter_frame(self,name):
request=self.codec.read_parameter(name)
def match(frame):
if frame.extended or len(frame.data)!=8:return None
if isinstance(self.codec,P80Codec):
return frame if frame.can_id==request.can_id and frame.data[0]==request.data[0] else None
d=frame.data
return frame if (frame.can_id==request.can_id and d[:2]==bytes([self.codec.motor_id,request.data[1]])
and d[6:]==b'\x00\xff') else None
return self._query(request,match)
def read_parameter(self,name):
return self.codec.decode_parameter(self.read_parameter_frame(name),name)
def read_version(self):
if isinstance(self.codec,PA43Codec):return self.read_parameter('firmware_version')
# P80 version data layout is not documented; preserve the raw payload.
return self._query(self.codec.read_version(),lambda f:f.data if
not f.extended and len(f.data)==8 and f.can_id==self.codec.motor_id and f.data[0]==1 else None)
def query_state(self):
if isinstance(self.codec,P80Codec):
return dict(enabled=bool(self.read_parameter('enabled')),control_mode=self.read_parameter('control_mode'))
return self._query(self.codec.state_command('query'),self.codec.decode_feedback)
def _control_gate(self):
if self.closed:raise RuntimeError('Manual client closed')
if not self.allow_control or not self.protocol_verified:
raise PermissionError('Manual hardware writes require explicit control and standard-protocol verification')
def write_parameter(self,name,value):
with self.lock:
self._control_gate()
# Generic parameter writes must not change transport, identity,
# calibration, mechanical zero or safety protection settings.
allowed={'control_mode','position_kp','position_ki','position_kd','velocity_kp','velocity_ki',
'current_kp','current_ki','default_position_kp','default_position_kd',
'default_velocity_kp','default_velocity_ki'}
if name not in allowed:raise PermissionError('Use a commissioned motion API; generic write is restricted')
if name=='control_mode':
state=self.query_state()
if state['enabled'] or state.get('fault_bits',0):raise ValueError('Mode change requires confirmed disabled, fault-free state')
frame=self.codec.write_parameter(name,value)
if self.can_fd:frame=frame.as_fd(bitrate_switch=self.bitrate_switch)
self.transport.send(frame);self._log('tx',frame)
actual=self.read_parameter(name)
import math
if not math.isclose(actual,value,rel_tol=1e-5,abs_tol=1e-6):raise ValueError('Parameter readback mismatch')
return actual
def close(self):
# No automatic enable/disable/mode switch on this read client.
with self.lock:
if not self.closed:self.closed=True;self.transport.close()
+327
View File
@@ -0,0 +1,327 @@
"""P80/P50 and PA43 local manuals 260708: independent standard-frame codecs.
No I/O, automatic protocol/mode change, calibration, flash or OTA commands.
Communication ranges are explicit; they are not mechanical limits.
"""
from dataclasses import dataclass
from enum import IntEnum
import math
import struct
from .protocol import Frame
class P80Mode(IntEnum):
TORQUE=0
MIT=1
VELOCITY=2
PROFILE_VELOCITY=3
POSITION=4
PROFILE_POSITION=5
class PA43Mode(IntEnum):
SERVO_POSITION=1
MIT=2
VELOCITY=3
def _id(value, ceiling=255):
if type(value) is not int or not 1<=value<=ceiling:raise ValueError('Unicast motor ID required')
return value
def _finite(value):
if isinstance(value,bool) or not isinstance(value,(int,float)) or not math.isfinite(value):
raise ValueError('Finite numeric value required')
return value
@dataclass(frozen=True)
class CommunicationRanges:
position: tuple
velocity: tuple
torque: tuple
kp: tuple
kd: tuple
ki: tuple = None
def __post_init__(self):
for name in ('position','velocity','torque','kp','kd','ki'):
bounds=getattr(self,name)
if name=='ki' and bounds is None:continue
if not isinstance(bounds,(tuple,list)) or len(bounds)!=2:raise ValueError(f'Invalid {name} range')
lo,hi=map(_finite,bounds)
if lo>=hi or (name in ('kp','kd','ki') and lo<0):raise ValueError(f'Invalid {name} range')
object.__setattr__(self,name,(lo,hi))
def _encode(value,bounds,bits):
value=_finite(value)
if bounds is None or not bounds[0]<=value<=bounds[1]:raise ValueError('Value outside explicit communication range')
return int((value-bounds[0])*((1<<bits)-1)/(bounds[1]-bounds[0]))
def _decode(value,bounds,bits):
if bounds is None:raise ValueError('Explicit communication range required')
return bounds[0]+value*(bounds[1]-bounds[0])/((1<<bits)-1)
def _standard(can_id,payload):
return Frame(can_id,payload,extended=False)
@dataclass(frozen=True)
class ManualParameter:
name: str
read_code: int
write_code: object = None
fmt: str = 'f'
unit: str = ''
# CMD + scalar at data[1:]. P80 manual does not specify general scalar
# endianness, so callers must select it explicitly to encode/decode scalars.
P80_PARAMETERS={p.name:p for p in [
ManualParameter('torque',0x03,0x0c,unit='Nm'),
ManualParameter('current',0x04,0x08,unit='A'),
ManualParameter('velocity',0x05,0x09,unit='rad/s'),
ManualParameter('position',0x06,0x0a,unit='rad'),
ManualParameter('enabled',0x2b,0x2a,'B'),
ManualParameter('control_mode',0x55,0x07,'B'),
ManualParameter('bus_voltage',0x45,unit='V'),
ManualParameter('core_temperature',0x5e,unit='degC'),
ManualParameter('motor_temperature',0x5f,unit='degC'),
ManualParameter('inverter_temperature',0x60,unit='degC'),
ManualParameter('profile_position_velocity',0x1c,0x1f,unit='rad/s'),
ManualParameter('profile_position_acceleration',0x1d,0x20,unit='rad/s2'),
ManualParameter('profile_position_deceleration',0x1e,0x21,unit='rad/s2'),
ManualParameter('profile_velocity_velocity',0x22,0x25,unit='rad/s'),
ManualParameter('profile_velocity_acceleration',0x23,0x26,unit='rad/s2'),
ManualParameter('profile_velocity_deceleration',0x24,0x27,unit='rad/s2'),
ManualParameter('current_kp',0x15,0x0e),ManualParameter('current_ki',0x16,0x0f),
ManualParameter('velocity_kp',0x17,0x10),ManualParameter('velocity_ki',0x18,0x11),
ManualParameter('position_kp',0x19,0x12),ManualParameter('position_ki',0x1a,0x13),
ManualParameter('position_kd',0x1b,0x14),
ManualParameter('current_pid_output_limit',0x34,0x2e),
ManualParameter('velocity_pid_output_limit',0x36,0x30),
ManualParameter('position_pid_output_limit',0x38,0x32),
ManualParameter('current_input_limit',0x59,0x58,unit='A'),
ManualParameter('velocity_input_limit',0x5b,0x5a,unit='rad/s'),
ManualParameter('position_max',0x85,0x83,unit='rad'),
ManualParameter('position_min',0x86,0x84,unit='rad'),
ManualParameter('inverter_protect_temperature',0x62,0x61),
ManualParameter('motor_protect_temperature',0x6c,0x6b),
ManualParameter('core_protect_temperature',0x71,0x70),
ManualParameter('undervoltage',0x73,0x72),ManualParameter('overvoltage',0x75,0x74),
ManualParameter('overspeed',0x77,0x76),ManualParameter('overcurrent',0x79,0x78),
ManualParameter('mit_position_max',0xa8,0xa0),ManualParameter('mit_position_min',0xa9,0xa1),
ManualParameter('mit_velocity_max',0xaa,0xa2),ManualParameter('mit_velocity_min',0xab,0xa3),
ManualParameter('mit_torque_max',0xac,0xa4),ManualParameter('mit_torque_min',0xad,0xa5),
ManualParameter('mit_kp_max',0xae,0xa6),ManualParameter('mit_kd_max',0xaf,0xa7),
ManualParameter('alarm',0xfd,None,'I'),
]}
class P80Codec:
family='P80'
def __init__(self,motor_id,*,ranges=None,scalar_byteorder=None):
self.motor_id=_id(motor_id,128)
if scalar_byteorder not in (None,'<','>'):raise ValueError('Scalar byte order must be explicitly < or >')
if ranges is not None and not isinstance(ranges,CommunicationRanges):raise ValueError('Explicit CommunicationRanges required')
self.ranges=ranges;self.scalar_byteorder=scalar_byteorder
def _order(self,p):
if p.fmt=='B':return '<'
if self.scalar_byteorder is None:raise ValueError('P80 scalar byte order is not specified by local manual; select explicitly')
return self.scalar_byteorder
def read_parameter(self,name):
return _standard(self.motor_id,bytes([P80_PARAMETERS[name].read_code])+bytes(7))
def read_version(self):return _standard(self.motor_id,b'\x01'+bytes(7))
def write_parameter(self,name,value):
p=P80_PARAMETERS[name]
if p.write_code is None:raise ValueError('Read-only manual parameter')
_finite(value)
if name=='control_mode':
if type(value) not in (int,P80Mode):raise ValueError('Integer or P80Mode required')
P80Mode(value)
value=int(value)
if name=='enabled' and (type(value) is not int or value not in (0,1)):
raise ValueError('Enable value must be integer 0 or 1')
return _standard(self.motor_id,(bytes([p.write_code])+struct.pack(self._order(p)+p.fmt,value)).ljust(8,b'\0'))
def decode_parameter(self,frame,name):
p=P80_PARAMETERS[name]
if frame.extended or len(frame.data)!=8 or frame.can_id!=self.motor_id or frame.data[0]!=p.read_code:return None
value=struct.unpack(self._order(p)+p.fmt,frame.data[1:1+struct.calcsize(p.fmt)])[0]
_finite(value)
return value
def mit(self,position,velocity,kp,kd,torque):
r=self.ranges
if r is None:raise ValueError('Read and confirm communication ranges first')
p,v,k,d,t=[_encode(x,b,n) for x,b,n in [(position,r.position,16),(velocity,r.velocity,10),
(kp,r.kp,10),(kd,r.kd,10),(torque,r.torque,10)]]
# P80 p8 receiver code is internally consistent; the bit table is not.
return _standard(self.motor_id,bytes([0x0b,p>>8,p&255,v>>2,
((v&3)<<6)|(k>>4),((k&15)<<4)|(d>>6),((d&63)<<2)|(t>>8),t&255]))
def decode_feedback(self,frame):
if frame.extended or len(frame.data)!=8 or frame.can_id!=self.motor_id:return None
if frame.data[0] not in (0x08,0x09,0x0a,0x0b,0x96):return None
d=frame.data;r=self.ranges
p=(d[1]<<8)|d[2];v=(d[3]<<4)|(d[4]>>4);t=((d[4]&15)<<8)|d[5]
return dict(motor_id=self.motor_id,command=d[0],position_rad=_decode(p,r.position,16) if r else None,
velocity_rad_s=_decode(v,r.velocity,12) if r else None,torque_nm=_decode(t,r.torque,12) if r else None,
raw_position=p,raw_velocity=v,raw_torque=t,error_code=(d[6]<<8)|d[7])
def read_feedback(self):return _standard(self.motor_id,b'\x96'+bytes(7))
@staticmethod
def broadcast_mit(motor_commands,*,bitrate_switch=False):
"""Eight explicit (codec, q/v/Kp/Kd/tau tuple) slots; P80 p11 MIT12.
Restrict to IDs1..8: published modulo grouping code is inconsistent.
"""
if type(bitrate_switch) is not bool:raise ValueError('Explicit BRS selection required')
motor_commands=list(motor_commands)
if len(motor_commands)!=8:raise ValueError('All eight explicit slots required')
payload=b''
for mid,(codec,values) in enumerate(motor_commands,1):
if not isinstance(codec,P80Codec) or codec.motor_id!=mid or codec.ranges is None:
raise ValueError('Explicit codecs/ranges for IDs1..8 in order required')
if len(values)!=5:raise ValueError('q/v/Kp/Kd/torque required')
r=codec.ranges
p,v,k,d,t=[_encode(x,b,n) for x,b,n in zip(values,
(r.position,r.velocity,r.kp,r.kd,r.torque),(16,12,12,12,12))]
payload+=bytes([p>>8,p&255,v>>4,((v&15)<<4)|(k>>8),k&255,
d>>4,((d&15)<<4)|(t>>8),t&255])
return Frame(0,payload,extended=False,is_fd=True,fd_flags=int(bitrate_switch))
@staticmethod
def broadcast_commands(frames,*,bitrate_switch=False):
if type(bitrate_switch) is not bool:raise ValueError('Explicit BRS selection required')
frames=list(frames)
if len(frames)!=8:raise ValueError('All eight explicit slots required')
for mid,frame in enumerate(frames,1):
if frame.extended or frame.is_fd or frame.can_id!=mid or len(frame.data)!=8:
raise ValueError('Standard commands for IDs1..8 in order required')
return Frame(0xff,b''.join(f.data for f in frames),extended=False,is_fd=True,
fd_flags=int(bitrate_switch))
PA43_PARAMETERS={}
def _pa(name,index,fmt='f',writable=True):
PA43_PARAMETERS[name]=ManualParameter(name,index,index if writable else None,fmt)
_pa('firmware_version',10,'i',False);_pa('control_mode',11,'i')
for index,name in enumerate(('id_kp','id_ki','iq_kp','iq_ki','current_deadzone','velocity_deadzone',
'position_deadzone','current_integral_limit','velocity_integral_limit','torque_limit',
'electric_angle_offset','machine_angle_offset','position_min','position_max','velocity_min',
'velocity_max','kp_min','kp_max','kd_min','kd_max','ki_min','ki_max','torque_min','torque_max'),12):
_pa(name,index)
_pa('motor_id',36,'i');_pa('can_timeout',37,'i')
for index,name in enumerate(('default_position_kp','default_position_kd','default_velocity_kp',
'default_velocity_ki','acceleration','torque_slope'),38):_pa(name,index)
_pa('pole_pairs',44,'i');_pa('gear_ratio',45);_pa('torque_constant',46)
_pa('rotation_direction',47,'i');_pa('encoder_config',48,'i')
for index,name in enumerate(('overvoltage','undervoltage','phase_overcurrent','pcb_overtemperature',
'stuck_current','stuck_velocity','stuck_time'),49):_pa(name,index)
_pa('protection_switches',56,'i')
# p40 indexes57..60 and65 have no defined data type: not guessed.
for index,name in enumerate(('current_filter_bandwidth','velocity_filter_bandwidth','position_filter_bandwidth'),61):
_pa(name,index,'i')
_pa('v_calibration',64);_pa('motor_overtemperature',66);_pa('can_master',67,'i')
_pa('nonius_flag',68,'i',False);_pa('protocol_type',69,'i')
_pa('error_code',70,'i',False);_pa('error_information',71,'f',False)
for index,name in enumerate(('ld','lq','resistance','flux'),72):_pa(name,index,'f',False)
class PA43Codec:
family='PA43'
def __init__(self,motor_id,*,ranges=None,feedback_id=None):
self.motor_id=_id(motor_id)
if ranges is not None and not isinstance(ranges,CommunicationRanges):raise ValueError('Explicit CommunicationRanges required')
self.ranges=ranges
self.feedback_id=self.motor_id if feedback_id is None else feedback_id
if type(self.feedback_id) is not int or not 0<=self.feedback_id<=0x7ff:
raise ValueError('Configured CAN_MASTER must be a standard ID')
def read_parameter(self,name):
p=PA43_PARAMETERS[name]
return _standard(0x600+self.motor_id,bytes([0x67,p.read_code,0,0,0,0,4,0x76]))
def write_parameter(self,name,value):
p=PA43_PARAMETERS[name]
if p.write_code is None:raise ValueError('Read-only manual parameter')
_finite(value)
if name=='control_mode':
if type(value) not in (int,PA43Mode):raise ValueError('Integer or PA43Mode required')
PA43Mode(value)
value=int(value)
if p.fmt=='i' and type(value) is not int:raise ValueError('Integer parameter required')
return _standard(0x600+self.motor_id,bytes([0x67,p.write_code])+struct.pack('<'+p.fmt,value)+b'\x15\x76')
def decode_parameter(self,frame,name):
p=PA43_PARAMETERS[name];d=frame.data
if (frame.extended or frame.can_id!=0x600+self.motor_id or len(d)!=8
or d[:2]!=bytes([self.motor_id,p.read_code]) or d[6:]!=b'\x00\xff'):return None
value=struct.unpack('<'+p.fmt,d[2:6])[0];_finite(value)
return value
def state_command(self,action):
# p7/p8: FA is a genuine read-only query; FE stores flash and is omitted.
codes={'enable':0xfc,'disable':0xfd,'query':0xfa,'clear_error':0xfb}
return _standard(0x100+self.motor_id,bytes([255])*7+bytes([codes[action]]))
def _ranges(self):
if self.ranges is None:raise ValueError('Read and confirm communication ranges first')
return self.ranges
def mit(self,position,velocity,kp,kd,torque):
r=self._ranges()
p,v,k,d,t=[_encode(x,b,n) for x,b,n in [(position,r.position,16),(velocity,r.velocity,12),
(kp,r.kp,12),(kd,r.kd,12),(torque,r.torque,12)]]
return _standard(0x200+self.motor_id,bytes([p>>8,p&255,v>>4,
((v&15)<<4)|(k>>8),k&255,d>>4,((d&15)<<4)|(t>>8),t&255]))
def servo_position(self,position,velocity_limit,position_kp,position_kd,velocity_kp,velocity_kd,velocity_ki):
r=self._ranges()
if _finite(velocity_limit)<0:raise ValueError('Servo velocity field is a nonnegative LIMIT, not feedforward')
p=_encode(position,r.position,16)
fields=[_encode(x,b,8) for x,b in [(velocity_limit,r.velocity),(position_kp,r.kp),
(position_kd,r.kd),(velocity_kp,r.kp),(velocity_kd,r.kd),(velocity_ki,r.ki)]]
return _standard(0x200+self.motor_id,bytes([p>>8,p&255,*fields]))
def velocity(self,velocity,kp,kd,ki):
r=self._ranges()
v,k,d,i=[_encode(x,b,n) for x,b,n in [(velocity,r.velocity,16),(kp,r.kp,12),
(kd,r.kd,12),(ki,r.ki,12)]]
# p27/p29 rule takes precedence over contradictory all-zero example.
return _standard(0x200+self.motor_id,bytes([v>>8,v&255,k>>4,
((k&15)<<4)|(d>>8),d&255,i>>4,(i&15)<<4,0xac]))
def decode_feedback(self,frame):
if frame.extended or frame.can_id!=self.feedback_id or len(frame.data)!=8:return None
d=frame.data;r=self.ranges
p=(d[0]<<8)|d[1];v=(d[2]<<4)|(d[3]>>4);t=((d[3]&15)<<8)|d[4]
flags=(d[6]<<8)|d[7]
return dict(motor_id=self.motor_id,position_rad=_decode(p,r.position,16) if r else None,
velocity_rad_s=_decode(v,r.velocity,12) if r else None,torque_nm=_decode(t,r.torque,12) if r else None,
temperature_c=d[5]-40,enabled=bool(flags&1),fault_bits=flags&0xfffe,
raw_position=p,raw_velocity=v,raw_torque=t,raw_flags=flags)
def broadcast(self,frames,*,kind='control',bitrate_switch=False):
if kind not in ('control','state'):raise ValueError('Broadcast kind must be control or state')
if type(bitrate_switch) is not bool:raise ValueError('Explicit BRS flag required')
frames=list(frames)
if len(frames)!=8:raise ValueError('All eight explicit slots required; no implicit zero-filled motors')
base=0x200 if kind=='control' else 0x100
for mid,frame in enumerate(frames,1):
if frame.extended or frame.is_fd or frame.can_id!=base+mid or len(frame.data)!=8:
raise ValueError('Broadcast slots must correspond to IDs 1..8 in order')
return Frame(0x20 if kind=='control' else 0x10,b''.join(f.data for f in frames),
extended=False,is_fd=True,fd_flags=int(bitrate_switch))
+185
View File
@@ -0,0 +1,185 @@
"""Observed RobStride-compatible extended protocol; family names are not inferred."""
from dataclasses import dataclass, asdict
import math
import struct
@dataclass(frozen=True)
class Frame:
can_id: int
data: bytes
extended: bool = True
is_fd: bool = False
fd_flags: int = 0
def __post_init__(self):
if type(self.extended) is not bool or type(self.is_fd) is not bool:
raise ValueError('Explicit boolean frame format required')
ceiling=0x1fffffff if self.extended else 0x7ff
if type(self.can_id) is not int or not 0 <= self.can_id <= ceiling:
raise ValueError('Arbitration ID outside selected frame format')
lengths=(8,12,16,20,24,32,48,64) if self.is_fd else (8,)
if not isinstance(self.data, bytes) or len(self.data) not in lengths:
raise ValueError('Invalid data length for SDK frame format')
if type(self.fd_flags) is not int or self.fd_flags & ~3 or (not self.is_fd and self.fd_flags):
raise ValueError('Invalid CAN FD flags')
def as_fd(self, *, bitrate_switch=False):
"""Explicit classic-to-FD envelope conversion; does not change payload/ID."""
if type(bitrate_switch) is not bool:raise ValueError('Explicit BRS selection required')
return Frame(self.can_id,self.data,self.extended,True,int(bitrate_switch))
@dataclass(frozen=True)
class Parameter:
index: int
name: str
fmt: str = 'f'
unit: str = ''
writable: bool = False
PARAMETERS = {p.name:p for p in [
Parameter(0x7005,'control_mode','B'), Parameter(0x7019,'position',unit='rad'),
Parameter(0x701b,'velocity',unit='rad/s'), Parameter(0x701a,'current',unit='A'),
Parameter(0x701c,'bus_voltage',unit='V'), Parameter(0x302c,'torque',unit='Nm'),
Parameter(0x7016,'position_target',unit='rad',writable=True),
Parameter(0x7017,'speed_limit',unit='rad/s',writable=True),
Parameter(0x7018,'current_limit',unit='A'), Parameter(0x700b,'torque_limit',unit='Nm'),
Parameter(0x701e,'position_kp'), Parameter(0x701f,'velocity_kp'),
Parameter(0x7020,'velocity_ki'), Parameter(0x7024,'profile_velocity',unit='rad/s'),
Parameter(0x7025,'profile_acceleration',unit='rad/s2'),
Parameter(0x7028,'can_timeout','I',unit='firmware ticks'),
]}
BY_INDEX={p.index:p for p in PARAMETERS.values()}
FAULTS=('undervoltage','phase_overcurrent','overtemperature','encoder','stall','uncalibrated')
def command(kind, motor_id, host_id=253, data=bytes(8)):
if type(motor_id) is not int or not 1<=motor_id<=255 or not 0<=host_id<=255:
raise ValueError('Invalid device/host ID')
return Frame((kind<<24)|(host_id<<8)|motor_id, data)
def read_parameter(motor_id, parameter, host_id=253):
return command(17,motor_id,host_id,struct.pack('<H2xI',parameter.index,0))
@dataclass(frozen=True)
class OperationRanges:
"""Explicit encoding ranges, not mechanical limits or model defaults.
Creating a codec does not commission a device or authorize transmission.
"""
position: tuple
velocity: tuple
torque: tuple
kp: tuple
kd: tuple
def __post_init__(self):
for name in ('position','velocity','torque','kp','kd'):
bounds=getattr(self,name)
if (len(bounds)!=2 or any(isinstance(x,bool) or not math.isfinite(x) for x in bounds)
or bounds[0]>=bounds[1]):
raise ValueError(f'Invalid {name} communication range')
if name in ('kp','kd') and bounds[0]!=0:
raise ValueError('RobStride gain encoding starts at zero')
object.__setattr__(self,name,tuple(bounds))
def _encode_u16(value, bounds):
if isinstance(value,bool) or not math.isfinite(value) or not bounds[0]<=value<=bounds[1]:
raise ValueError('Value outside explicit communication range or nonfinite')
return int((value-bounds[0])*65535/(bounds[1]-bounds[0]))
def _decode_u16(raw, bounds):
return bounds[0]+raw*(bounds[1]-bounds[0])/65535
def operation_command(motor_id, position_rad, velocity_rad_s, kp, kd,
feedforward_nm, *, ranges):
"""Pure type-1 extended operation-mode encoder; no CAN I/O.
ID bits23..8 hold torque, NOT host ID. Data: q/v/Kp/Kd, each big-endian
uint16. This is the private extended protocol, not standard MIT framing.
"""
if type(motor_id) is not int or not 1<=motor_id<=255:
raise ValueError('Unicast motor ID required')
if not isinstance(ranges,OperationRanges):raise ValueError('Explicit OperationRanges required')
torque=_encode_u16(feedforward_nm,ranges.torque)
payload=struct.pack('>4H',*[_encode_u16(value,bounds) for value,bounds in
[(position_rad,ranges.position),(velocity_rad_s,ranges.velocity),(kp,ranges.kp),(kd,ranges.kd)]])
return Frame((1<<24)|(torque<<8)|motor_id,payload)
def decode_operation(frame, *, ranges):
"""Offline inverse for auditing; never interpreted as measured state."""
if not frame.extended or frame.is_fd or frame.can_id>>24!=1:return None
if not isinstance(ranges,OperationRanges):raise ValueError('Explicit OperationRanges required')
q,v,kp,kd=struct.unpack('>4H',frame.data)
return dict(motor_id=frame.can_id&255,position_rad=_decode_u16(q,ranges.position),
velocity_rad_s=_decode_u16(v,ranges.velocity),kp=_decode_u16(kp,ranges.kp),
kd=_decode_u16(kd,ranges.kd),feedforward_nm=_decode_u16((frame.can_id>>8)&65535,ranges.torque))
def write_parameter(motor_id, parameter, value, host_id=253):
if not parameter.writable or not math.isfinite(value):
raise ValueError('Parameter not writable/finite')
return command(18,motor_id,host_id,struct.pack('<H2x',parameter.index)+struct.pack('<'+parameter.fmt,value).ljust(4,b'\0'))
@dataclass(frozen=True)
class MotorState:
motor_id: int
host_id: int
timestamp: float
position_rad: float
temperature_c: float
mode_state: int
fault_bits: int
raw_position: int
raw_velocity: int
raw_torque: int
arbitration_id: int
payload_hex: str
velocity_rad_s: object = None
torque_nm: object = None
current_a: object = None
bus_voltage_v: object = None
fault_report: int = 0
warning_report: int = 0
@property
def enabled(self): return self.mode_state == 2
@property
def faults(self): return [name for bit,name in enumerate(FAULTS) if self.fault_bits & (1<<bit)]
def to_dict(self): return dict(asdict(self), enabled=self.enabled, faults=self.faults)
def decode_state(frame, timestamp, position_range=(-12.57,12.57), *, velocity_range=None, torque_range=None):
if not frame.extended or frame.is_fd:return None
kind=frame.can_id>>24
if kind not in (2,24):return None
p,v,t,temp=struct.unpack('>4H',frame.data)
lo,hi=position_range
if not math.isfinite(lo+hi) or lo>=hi:raise ValueError('Invalid position range')
# Optional physical decoding is only enabled with explicit caller ranges.
# Existing real-device path leaves both values unknown.
for bounds in (velocity_range,torque_range):
if bounds is not None and (len(bounds)!=2 or not math.isfinite(sum(bounds)) or bounds[0]>=bounds[1]):
raise ValueError('Invalid explicit feedback range')
return MotorState((frame.can_id>>8)&255,frame.can_id&255,timestamp,
lo+p/65535*(hi-lo),temp/10.,(frame.can_id>>22)&3,
(frame.can_id>>16)&63,p,v,t,frame.can_id,frame.data.hex(),
velocity_rad_s=_decode_u16(v,velocity_range) if velocity_range is not None else None,
torque_nm=_decode_u16(t,torque_range) if torque_range is not None else None)
def decode_parameter(frame):
if not frame.extended or frame.is_fd or frame.can_id>>24 != 17:return None
index=struct.unpack('<H',frame.data[:2])[0]
p=BY_INDEX.get(index)
status=(frame.can_id>>16)&255
value=struct.unpack('<'+p.fmt,frame.data[4:4+struct.calcsize(p.fmt)])[0] if p and status==0 else None
if isinstance(value,float) and not math.isfinite(value):raise ValueError('Nonfinite parameter response')
return (frame.can_id>>8)&255,frame.can_id&255,index,status,value
+168
View File
@@ -0,0 +1,168 @@
"""ROS-neutral trajectory adapter for the observed extended protocol."""
from dataclasses import dataclass
import math
import threading
import time
from .client import MotorBus
from .protocol import PARAMETERS
@dataclass
class JointState:
position: float
velocity: object
effort: object
feedback_timestamp_s: float = 0.
class Runtime:
def __init__(self, transport, cfg, bounds, mode='monitor'):
self.cfg=cfg;self.motors=cfg['motors'];self.bounds=bounds;self.mode=mode
self.bus=MotorBus(transport,self.motors,allow_control=mode in ('fake','hardware'))
self.running=False;self.max_tx_gap=0.;self.fault='';self.unsupported=[]
self.max_velocity=cfg['max_velocity_rad_s'];self.max_acceleration=cfg['max_acceleration_rad_s2']
self.quit=threading.Event();self.lock=threading.RLock();self.threads=[]
self.spline=None;self.duration=0.;self.started=0.;self.target=None
self.trace=None;self.trajectory_sequence=0
if cfg.get("trace_directory"):
from .trace import ExecutionTrace
self.trace=ExecutionTrace(cfg["trace_directory"],dict(mode=mode,joint_names=[m["joint"] for m in self.motors]))
def start(self):
self.bus.start()
self.threads=[threading.Thread(target=f,daemon=True,name=n) for f,n in
[(self._poll,'a7_can_parameters'),(self._control,'a7_can_control')]]
for t in self.threads:t.start()
def snapshot(self, require_enabled=False):
if require_enabled and (not self.running or self.fault or self.bus.fault):raise ValueError(self.fault or self.bus.fault or 'Output disabled')
rows=[]
for m in self.motors:
s=self.bus.get_state(m['motor_id'])
if require_enabled and (not s.enabled or s.fault_bits or s.fault_report):raise ValueError('Motor disabled/faulted')
rows.append(JointState(m['sign']*s.position_rad+m['offset_rad'],
None if s.velocity_rad_s is None else m['sign']*s.velocity_rad_s,
None if s.torque_nm is None else m['sign']*s.torque_nm,s.timestamp))
return rows
def diagnostics(self):
return dict(self.bus.diagnostics(),execution_trace=None if self.trace is None else dict(path=str(self.trace.path),dropped=self.trace.dropped,error=self.trace.error),protocol='a7_extended_v1',runtime_fault=self.fault,
mapping_verified=self.mode=='fake' or self.cfg['verified']['zero_sign_verified'],
mapping_source=self.cfg.get('mapping_source',{}),
static_scene_verified=self.mode=='fake' or self.cfg['verified']['static_scene_verified'],
unsupported_parameters=list(self.unsupported),
unmeasured_joints=list(self.cfg['static_positions']),
joint_velocity_effort_source='parameter queries; missing values omitted, no guessed packed scale')
def _poll(self):
try:
for m in self.motors:self.bus.identify(m['motor_id'])
except Exception as exc:self.fault=f'Identity verification: {exc}';return
failed=set()
for m in self.motors:
for name in PARAMETERS:
if name in ('velocity','current','bus_voltage','torque'):continue
try:self.bus.read_parameter(m['motor_id'],name,timeout=.05)
except (NotImplementedError,TimeoutError) as exc:
self.unsupported.append(f'{(m["motor_id"],name)}: {exc}')
except Exception as exc:self.fault=f'Telemetry: {exc}';return
if self.quit.is_set():return
while not self.quit.is_set():
for m in self.motors:
for name in ('velocity','current','bus_voltage','torque'):
key=(m['motor_id'],name)
if key in failed:continue
try:self.bus.read_parameter(*key,timeout=.05)
except (NotImplementedError,TimeoutError) as exc:
failed.add(key);self.unsupported.append(f'{key}: {exc}')
except Exception as exc:self.fault=f'Telemetry: {exc}';return
if self.quit.wait(.004):return
if self.quit.wait(.05):return
def enable(self):
if self.mode=='monitor':raise PermissionError('Monitor mode')
with self.lock:
if self.running:raise ValueError('Already enabled')
if self.fault or self.bus.fault:raise ValueError(self.fault or self.bus.fault)
rows=self.snapshot()
if any(not lo<=s.position<=hi for s,(lo,hi) in zip(rows,self.bounds)):raise ValueError('Measured joints outside URDF limits')
positions=self.bus.enable(speed_limit=self.max_velocity,
apply_configured_speed_limit=self.cfg.get('apply_configured_speed_limit',False))
self.target=[m['sign']*positions[m['motor_id']]+m['offset_rad'] for m in self.motors]
self.spline=None;self.running=True
def follow(self,spline,duration):
with self.lock:
self.snapshot(True)
self.spline=spline;self.duration=duration;self.started=time.monotonic()
self.trajectory_sequence+=1
if self.trace:self.trace.emit(dict(event="follow",sequence=self.trajectory_sequence,monotonic_s=self.started,duration_s=duration,effective_speed_limits_rad_s=dict(self.bus.effective_speed_limits)))
def finish(self):
with self.lock:
if self.spline is not None:self.target=list(self.spline(self.duration))
self.spline=None
def hold_current(self):
"""Cancel the trajectory, immediately command measured pose, keep enabled.
This is a position hold, not a guaranteed acceleration-limited brake.
Failed feedback/write propagates to caller's fault-disable path.
"""
with self.lock:
rows = self.snapshot(True)
target = [r.position for r in rows]
if any(not lo <= q <= hi for q, (lo, hi) in zip(target, self.bounds)):
raise ValueError('Cannot hold measured position outside limits')
self.spline = None
self.target = target
self.bus.set_positions({m['motor_id']: (q-m['offset_rad'])/m['sign']
for m, q in zip(self.motors, target)})
return list(target)
def stop(self,reason='Operator stop'):
with self.lock:
self.running=False;self.spline=None
return self.bus.disable()
def _control(self):
previous=None;deadline=time.monotonic()
while not self.quit.is_set():
try:
with self.lock:
if self.running:
now=time.monotonic()
if previous is not None:
gap=now-previous;self.max_tx_gap=max(self.max_tx_gap,gap)
if gap>.12:raise ValueError('CAN control deadline missed')
previous=now;rows=self.snapshot(True)
target=list(self.spline(min(now-self.started,self.duration))) if self.spline is not None else self.target
if any(not math.isfinite(q) or not lo<=q<=hi for q,(lo,hi) in zip(target,self.bounds)):raise ValueError('Trajectory outside URDF bounds')
if any(abs(q-s.position)>.08 for q,s in zip(target,rows)):raise ValueError('Trajectory tracking error >0.08 rad')
send_start=time.monotonic()
sends=self.bus.set_positions({m['motor_id']:(q-m['offset_rad'])/m['sign'] for m,q in zip(self.motors,target)})
if self.trace:self.trace.emit(dict(event='cycle',sequence=self.trajectory_sequence,
planned_cycle_s=deadline,cycle_start_s=now,send_start_s=send_start,send_end_s=time.monotonic(),
trajectory_elapsed_s=now-self.started if self.trajectory_sequence else None,
phase='trajectory' if self.spline is not None else 'hold',
target_rad=target,actual_rad=[r.position for r in rows],
feedback_timestamp_s=[r.feedback_timestamp_s for r in rows],
feedback_age_s=[now-r.feedback_timestamp_s for r in rows],sends=sends))
else:previous=None
except Exception as exc:
self.fault=str(exc)
if self.trace:self.trace.emit(dict(event="fault",monotonic_s=time.monotonic(),reason=str(exc)))
try:
if not self.stop(self.fault):self.fault+='; stop not acknowledged'
except Exception as stop_exc:self.fault+=f'; stop failed: {stop_exc}'
deadline+=.02
delay=deadline-time.monotonic()
if delay<0:deadline=time.monotonic();delay=0
self.quit.wait(delay)
def close(self):
self.quit.set()
for t in self.threads:t.join(timeout=2.)
try:return self.bus.close()
finally:
if self.trace:self.trace.close()
+40
View File
@@ -0,0 +1,40 @@
"""One-shot startup enable after feedback/identity readiness; no retry on failure."""
import time
class StartupEnable:
def __init__(self, runtime, requested=False, mode='monitor', timeout=5.):
if requested and mode not in ('fake', 'hardware'):
raise ValueError('Automatic enable requires fake or hardware mode')
self.runtime = runtime
self.deadline = time.monotonic() + timeout
self.state = 'waiting' if requested else 'off'
self.error = ''
def cancel(self):
if self.state == 'waiting':
self.state = 'cancelled'
def step(self):
"""Caller serializes this with operator commands. Returns current state."""
if self.state != 'waiting':
return self.state
try:
if self.runtime.fault or self.runtime.bus.fault:
raise RuntimeError(self.runtime.fault or self.runtime.bus.fault)
try:
self.runtime.snapshot()
ready = len(self.runtime.bus.diagnostics()['identities']) == len(self.runtime.motors)
except ValueError:
ready = False
if not ready:
if time.monotonic() >= self.deadline:
raise TimeoutError('Startup timed out waiting for all motor identities and fresh feedback')
return self.state
self.state = 'enabling'
self.runtime.enable()
self.state = 'enabled'
except Exception as exc:
self.error = str(exc)
self.state = 'failed'
return self.state
+32
View File
@@ -0,0 +1,32 @@
"""Bounded asynchronous JSONL writer; no disk I/O on the control thread."""
import json
import queue
import threading
import time
from pathlib import Path
from datetime import datetime
class ExecutionTrace:
def __init__(self, directory, metadata):
folder=Path(directory);folder.mkdir(parents=True,exist_ok=True)
self.path=folder/(datetime.now().strftime('%Y%m%d_%H%M%S_%f')+'.jsonl')
self.queue=queue.Queue(maxsize=10000);self.dropped=0;self.error='';self.done=threading.Event()
self.thread=threading.Thread(target=self._write,daemon=True,name='a7_trace_writer')
self.emit(dict(event='session',wall_time_ns=time.time_ns(),monotonic_s=time.monotonic(),
timestamp_meaning='host monotonic; send end means SocketCAN enqueue, not motor acknowledgement',**metadata))
self.thread.start()
def emit(self,row):
try:self.queue.put_nowait(row)
except queue.Full:self.dropped+=1
def _write(self):
try:
with self.path.open('x') as f:
while not self.done.is_set() or not self.queue.empty():
try:row=self.queue.get(timeout=.1)
except queue.Empty:f.flush();continue
f.write(json.dumps(row,allow_nan=False)+'\n')
f.write(json.dumps(dict(event='trace_closed',dropped=self.dropped))+'\n')
except Exception as exc:self.error=str(exc)
def close(self):
self.done.set();self.thread.join(timeout=3.)
if self.thread.is_alive():self.error='Trace writer did not finish within 3 seconds'
+62
View File
@@ -0,0 +1,62 @@
"""Exclusive extended classic SocketCAN transport. No automatic motor commands."""
import fcntl
import json
import select
import socket
import struct
import subprocess
from .protocol import Frame
class SocketCan:
def __init__(self, interface='can0', bitrate=1000000, *, frame_format='extended', fd=False,
data_bitrate=None):
if frame_format not in ('extended','standard','mixed'):raise ValueError('Invalid frame format')
if type(fd) is not bool:raise ValueError('Explicit CAN FD selection required')
if not interface or not all(c.isalnum() or c in '_-' for c in interface):raise ValueError('Invalid interface')
info=json.loads(subprocess.check_output(['ip','-details','-json','link','show',interface]))[0]
cfg=info.get('linkinfo',{}).get('info_data',{})
if (info.get('link_type')!='can' or 'UP' not in info['flags'] or info['mtu']!=(72 if fd else 16) or
cfg.get('bittiming',{}).get('bitrate')!=bitrate or 'LISTEN-ONLY' in cfg.get('ctrlmode',[])):
raise ValueError('Requires UP CAN with selected format and configured bitrate')
if fd and (not data_bitrate or cfg.get('data_bittiming',{}).get('bitrate')!=data_bitrate):
raise ValueError('CAN FD requires explicit verified data bitrate; interface is never reconfigured')
self.frame_format=frame_format;self.fd=fd
self.sock=None;self.lock=None
try:
self.lock=open(f'/tmp/a7_direct_{interface}.lock','a')
fcntl.flock(self.lock,fcntl.LOCK_EX|fcntl.LOCK_NB)
self.sock=socket.socket(socket.PF_CAN,socket.SOCK_RAW,socket.CAN_RAW)
if fd:self.sock.setsockopt(socket.SOL_CAN_RAW,getattr(socket,'CAN_RAW_FD_FRAMES',5),1)
self.sock.setsockopt(socket.SOL_CAN_RAW,socket.CAN_RAW_RECV_OWN_MSGS,0)
self.sock.setsockopt(socket.SOL_CAN_RAW,getattr(socket,'CAN_RAW_ERR_FILTER',2),struct.pack('=I',0x1fffffff))
self.sock.bind((interface,));self.sock.setblocking(False)
except BaseException:self.close();raise
def send(self, frame):
if self.frame_format!='mixed' and frame.extended!=(self.frame_format=='extended'):
raise ValueError('Frame format differs from explicitly selected transport')
if frame.is_fd and not self.fd:raise ValueError('CAN FD not enabled on this transport')
ident=frame.can_id|(0x80000000 if frame.extended else 0)
data=(struct.pack('=IBB2x64s',ident,len(frame.data),frame.fd_flags,frame.data) if frame.is_fd
else struct.pack('=IB3x8s',ident,len(frame.data),frame.data))
if self.sock.send(data)!=len(data):raise OSError('Incomplete CAN send')
def receive(self, timeout=.01):
if not select.select([self.sock],[],[],timeout)[0]:return None
raw,_,flags,_=self.sock.recvmsg(72)
if len(raw) not in (16,72) or flags&socket.MSG_TRUNC:raise OSError('Invalid CAN frame')
is_fd=len(raw)==72
if is_fd:
ident,n,fd_flags,data=struct.unpack('=IBB2x64s',raw)
else:
ident,n,data=struct.unpack('=IB3x8s',raw);fd_flags=0
if ident&0x20000000:raise OSError(f'CAN error {ident:#x}: {data.hex()}')
extended=bool(ident&0x80000000)
if ident&0x40000000:return None
if self.frame_format!='mixed' and extended!=(self.frame_format=='extended'):return None
if is_fd and not self.fd:return None
if n not in ((8,12,16,20,24,32,48,64) if is_fd else (8,)):return None
# Linux CANFD_FDF is a kernel marker, not a transmit BRS/ESI flag.
return Frame(ident&0x1fffffff,data[:n],extended,is_fd,fd_flags&3)
def close(self):
if self.sock is not None:self.sock.close();self.sock=None
if self.lock is not None:self.lock.close();self.lock=None
+172
View File
@@ -0,0 +1,172 @@
import queue
import struct
import unittest
from unittest.mock import Mock,patch
from a7_can_sdk.protocol import Frame,decode_state,decode_parameter
from a7_can_sdk.manual_protocol import CommunicationRanges,P80Codec,PA43Codec,P80Mode,PA43Mode
from a7_can_sdk.manual_client import ManualClient
from a7_can_sdk.transport import SocketCan
RANGES=CommunicationRanges((-1,1),(-1,1),(-1,1),(0,500),(0,5),(0,100))
class ManualTests(unittest.TestCase):
def test_family_specific_modes_and_read_vectors(self):
self.assertEqual(int(P80Mode.MIT),1);self.assertEqual(int(PA43Mode.MIT),2)
a=P80Codec(63).read_parameter('control_mode')
self.assertEqual((a.can_id,a.data.hex(),a.extended),(0x3f,'5500000000000000',False))
b=PA43Codec(67).read_parameter('control_mode')
self.assertEqual((b.can_id,b.data.hex(),b.extended),(0x643,'670b000000000476',False))
self.assertIsNone(decode_state(a,0));self.assertIsNone(decode_parameter(b))
def test_p80_ten_bit_matches_manual_receiver(self):
c=P80Codec(63,ranges=RANGES)
self.assertEqual(c.mit(0,1,0,0,0).data.hex(),'0b7fffffc00001ff')
for i in range(101):
fraction=i/100
f=c.mit(2*fraction-1,2*fraction-1,500*fraction,5*fraction,2*fraction-1)
d=f.data[1:]
raw=[(d[0]<<8)|d[1],(d[2]<<2)|(d[3]>>6),((d[3]&63)<<4)|(d[4]>>4),
((d[4]&15)<<6)|(d[5]>>2),((d[5]&3)<<8)|d[6]]
for actual,maximum in zip(raw,[65535,1023,1023,1023,1023]):
self.assertLessEqual(abs(actual-fraction*maximum),1.000001)
def test_pa43_mit_servo_velocity_vectors(self):
c=PA43Codec(67,ranges=RANGES)
self.assertEqual(c.mit(0,0,0,0,0).data.hex(),'7fff7ff0000007ff')
self.assertEqual(c.velocity(0,0,0,0).data.hex(),'7fff0000000000ac')
self.assertEqual(c.servo_position(0,0,0,0,0,0,0).data.hex(),'7fff7f0000000000')
self.assertEqual(c.mit(1,1,500,5,1).data,b'\xff'*8)
self.assertEqual(c.state_command('query').data.hex(),'fffffffffffffffa')
self.assertEqual(c.state_command('query').can_id,0x143)
def test_feedback_flags_and_physical_endpoints(self):
p=P80Codec(61,ranges=RANGES)
row=p.decode_feedback(Frame(61,bytes.fromhex('0bffffffffffffff'),extended=False))
self.assertEqual((row['position_rad'],row['velocity_rad_s'],row['torque_nm'],row['error_code']),(1,1,1,65535))
c=PA43Codec(67,ranges=RANGES,feedback_id=253)
row=c.decode_feedback(Frame(253,bytes.fromhex('ffffffffffff2001'),extended=False))
self.assertEqual((row['position_rad'],row['velocity_rad_s'],row['torque_nm']),(1,1,1))
self.assertEqual(row['temperature_c'],215);self.assertTrue(row['enabled'])
self.assertEqual(row['fault_bits'],0x2000)
self.assertIsNone(c.decode_feedback(Frame(67,bytes(8),extended=False)))
raw=PA43Codec(67).decode_feedback(Frame(67,bytes(8),extended=False))
self.assertIsNone(raw['position_rad']);self.assertFalse(raw['enabled'])
def test_pa43_parameter_endianness_and_reply(self):
c=PA43Codec(6)
# Exact IEEE754 parameter vector from local manual p45.
self.assertEqual(c.write_parameter('id_kp',.3).data.hex(),'670c9a99993e1576')
frame=Frame(0x606,bytes.fromhex('060c9a99993e00ff'),extended=False)
self.assertAlmostEqual(c.decode_parameter(frame,'id_kp'),.3,places=7)
self.assertIsNone(c.decode_parameter(frame,'iq_kp'))
self.assertEqual(c.write_parameter('control_mode',3).data.hex(),'670b030000001576')
self.assertEqual(c.write_parameter('control_mode',PA43Mode.MIT).data.hex(),'670b020000001576')
with self.assertRaises(ValueError):c.write_parameter('control_mode',P80Mode.MIT)
for name in ('firmware_version','error_code','nonius_flag'):
with self.assertRaises(ValueError):c.write_parameter(name,0)
def test_p80_unspecified_scalar_byteorder_never_guessed(self):
c=P80Codec(61)
with self.assertRaises(ValueError):c.write_parameter('position',.5)
with self.assertRaises(ValueError):c.decode_parameter(Frame(61,b'\x06'+bytes(7),extended=False),'position')
self.assertEqual(P80Codec(61,scalar_byteorder='<').write_parameter('position',.5).data.hex(),'0a0000003f000000')
self.assertEqual(c.write_parameter('control_mode',1).data.hex(),'0701000000000000')
self.assertEqual(c.write_parameter('control_mode',P80Mode.MIT).data.hex(),'0701000000000000')
with self.assertRaises(ValueError):c.write_parameter('control_mode',PA43Mode.MIT)
def test_invalid_inputs_reject_before_transmission(self):
for mid in (0,129,255,True):
with self.assertRaises(ValueError):P80Codec(mid)
for mid in (0,256,True):
with self.assertRaises(ValueError):PA43Codec(mid)
c=PA43Codec(67,ranges=RANGES)
for value in (float('nan'),float('inf'),2,True):
with self.assertRaises(ValueError):c.mit(value,0,0,0,0)
with self.assertRaises(ValueError):c.servo_position(0,-.1,0,0,0,0,0)
with self.assertRaises(ValueError):PA43Codec(67).mit(0,0,0,0,0)
with self.assertRaises(ValueError):Frame(0x800,bytes(8),extended=False)
with self.assertRaises(ValueError):Frame(1,bytes(64),extended=False)
def test_fd_broadcast_requires_all_eight_valid_slots(self):
frames=[PA43Codec(i,ranges=RANGES).mit(0,0,0,0,0) for i in range(1,9)]
f=PA43Codec(1).broadcast(frames,bitrate_switch=True)
self.assertEqual((f.can_id,len(f.data),f.is_fd,f.extended,f.fd_flags),(0x20,64,True,False,1))
with self.assertRaises(ValueError):PA43Codec(1).broadcast(frames[:7])
with self.assertRaises(ValueError):PA43Codec(1).broadcast(frames[::-1])
with self.assertRaises(ValueError):PA43Codec(1).broadcast([PA43Codec(67,ranges=RANGES).mit(0,0,0,0,0)]*8)
def test_p80_fd_broadcast_uses_twelve_not_ten_bit_payload(self):
pairs=[(P80Codec(i,ranges=RANGES),(0,0,0,0,0)) for i in range(1,9)]
frame=P80Codec.broadcast_mit(pairs)
self.assertEqual((frame.can_id,frame.is_fd,frame.extended),(0,True,False))
self.assertEqual(frame.data,bytes.fromhex('7fff7ff0000007ff')*8)
commands=[P80Codec(i).read_parameter('enabled') for i in range(1,9)]
self.assertEqual(P80Codec.broadcast_commands(commands).can_id,255)
with self.assertRaises(ValueError):P80Codec.broadcast_mit(pairs[::-1])
def test_manual_client_explicit_fd_envelope(self):
class Peer:
def __init__(self):self.sent=[];self.pending=None
def send(self,f):
self.sent.append(f)
self.pending=Frame(0x643,bytes.fromhex('430b0100000000ff'),extended=False,is_fd=True)
def receive(self,timeout):
f=self.pending;self.pending=None;return f
def close(self):pass
peer=Peer();client=ManualClient(peer,PA43Codec(67),can_fd=True,bitrate_switch=True)
self.assertEqual(client.read_parameter('control_mode'),1)
self.assertTrue(peer.sent[0].is_fd);self.assertEqual(peer.sent[0].fd_flags,1)
def test_transport_keeps_standard_fd_and_extended_distinct(self):
t=SocketCan.__new__(SocketCan);t.sock=Mock();t.lock=None;t.fd=True;t.frame_format='mixed'
t.sock.send.side_effect=lambda raw:len(raw)
a=P80Codec(61).read_parameter('enabled');t.send(a)
raw=t.sock.send.call_args.args[0]
self.assertEqual(len(raw),16);self.assertEqual(struct.unpack('=I',raw[:4])[0],61)
ext=Frame(0x1100fd3d,bytes(8));t.send(ext)
self.assertEqual(struct.unpack('=I',t.sock.send.call_args.args[0][:4])[0],0x9100fd3d)
fd=Frame(0x20,bytes(range(64)),extended=False,is_fd=True,fd_flags=1);t.send(fd)
raw=t.sock.send.call_args.args[0];self.assertEqual(len(raw),72)
t.sock.recvmsg.return_value=(raw,[],0,None)
with patch('a7_can_sdk.transport.select.select',return_value=([t.sock],[],[])):
self.assertEqual(t.receive(),fd)
t.fd=False
with self.assertRaises(ValueError):t.send(fd)
t.frame_format='extended'
with self.assertRaises(ValueError):t.send(a)
def test_manual_client_accepts_valid_zero_value_reply_and_is_readonly(self):
class Peer:
def __init__(self):self.sent=[];self.queue=queue.Queue()
def send(self,f):self.sent.append(f);self.queue.put(f)
def receive(self,timeout):
try:return self.queue.get(timeout=timeout)
except queue.Empty:return None
def close(self):pass
peer=Peer();client=ManualClient(peer,P80Codec(61),timeout=.02)
# Mode0 response equals request bytes; equality is not evidence of echo.
self.assertEqual(client.read_parameter('control_mode'),0)
count=len(peer.sent)
with self.assertRaises(PermissionError):client.write_parameter('control_mode',1)
self.assertEqual(len(peer.sent),count)
client.close()
with self.assertRaises(RuntimeError):client.read_parameter('control_mode')
def test_manual_client_mode_write_rejects_enabled_motor(self):
class Peer:
def __init__(self):self.sent=[];self.queue=queue.Queue()
def send(self,f):
self.sent.append(f)
if f.can_id==0x143:self.queue.put(Frame(67,bytes.fromhex('7fff7ff7ff410001'),extended=False))
def receive(self,timeout):
try:return self.queue.get(timeout=timeout)
except queue.Empty:return None
def close(self):pass
peer=Peer();client=ManualClient(peer,PA43Codec(67),allow_control=True,protocol_verified=True,timeout=.02)
with self.assertRaises(ValueError):client.write_parameter('control_mode',2)
self.assertEqual(len(peer.sent),1);self.assertEqual(peer.sent[0].data[-1],0xfa)
if __name__=='__main__':unittest.main()
+117
View File
@@ -0,0 +1,117 @@
import math
from pathlib import Path
import struct
import sys
import time
import unittest
sys.path.insert(0,str(Path(__file__).resolve().parents[1]))
from a7_can_sdk import MotorBus, NotCommissionedError
from a7_can_sdk.fake import FakeCan
from a7_can_sdk.protocol import Frame, PARAMETERS, command, decode_state, read_parameter
class SDKTests(unittest.TestCase):
def setUp(self):
self.motors=[dict(motor_id=i,host_id=253,uid=f'{i:016x}',raw_limits=[-2.,2.]) for i in (61,62)]
self.peer=FakeCan(self.motors,[.1,.2]);self.bus=MotorBus(self.peer,self.motors,allow_control=True).start()
time.sleep(.03)
def tearDown(self):self.bus.close()
def test_extended_golden_vector_and_state(self):
f=read_parameter(67,PARAMETERS['position'])
self.assertEqual(f.can_id,0x1100fd43);self.assertEqual(f.data.hex(),'1970000000000000')
s=decode_state(Frame(0x028243fd,struct.pack('>4H',32768,13,24,335)),123.)
self.assertTrue(s.enabled);self.assertEqual(s.faults,['phase_overcurrent'])
self.assertEqual(s.temperature_c,33.5);self.assertEqual(s.raw_velocity,13)
self.assertIsNone(s.velocity_rad_s);self.assertIsNone(s.torque_nm)
def test_read_only_and_force_no_commands(self):
self.bus.allow_control=False
self.assertEqual(self.bus.identify(61),self.motors[0]['uid'])
self.assertAlmostEqual(self.bus.read_parameter(61,'position'),.1)
count=len(self.peer.frames)
for fn in (self.bus.enable,self.bus.disable,lambda:self.bus.set_positions({61:.2}),lambda:self.bus.write_parameter(61,'position_target',.2)):
with self.assertRaises(PermissionError):fn()
with self.assertRaises(NotCommissionedError):self.bus.set_torque(61,1.)
with self.assertRaises(NotCommissionedError):self.bus.set_impedance(61,0.,0.,1.,1.)
self.assertEqual(len(self.peer.frames),count)
def test_typed_queries_missing_and_stale_optional_values(self):
self.assertEqual(self.bus.read_parameter(61,'control_mode'),5)
self.assertEqual(self.bus.read_parameter(61,'bus_voltage'),48.)
self.assertEqual(self.bus.get_state(61).bus_voltage_v,48.)
self.peer.unsupported.add((61,0x302c))
with self.assertRaises(NotImplementedError):self.bus.read_parameter(61,'torque')
time.sleep(.61)
self.assertIsNone(self.bus.get_state(61).bus_voltage_v)
def test_enable_preload_move_stop(self):
self.bus.enable();self.assertEqual(self.peer.enabled,{61,62})
kinds=[f.can_id>>24 for f in self.peer.frames]
self.assertGreater(kinds.index(3),kinds.index(18))
self.bus.set_positions({61:.102,62:.202});time.sleep(.15)
self.assertAlmostEqual(self.bus.get_state(61).position_rad,.102,delta=.0004)
self.assertTrue(self.bus.disable());self.assertFalse(self.peer.enabled)
def test_velocity_telemetry_does_not_block_enable(self):
original = self.bus.read_parameter
def read(mid, name, *args, **kwargs):
return .1 if name == 'velocity' else original(mid, name, *args, **kwargs)
self.bus.read_parameter = read
self.bus.enable()
self.assertEqual(self.peer.enabled, {61, 62})
for mid, q in self.peer.positions.items():
self.assertAlmostEqual(self.peer.targets[mid], q, places=6)
def test_explicit_speed_setting_replaces_old_lower_cap(self):
self.bus.enable(speed_limit=math.radians(4), apply_configured_speed_limit=True)
for mid in (61,62):
self.assertAlmostEqual(self.bus.read_parameter(mid,'speed_limit'),math.radians(4),places=7)
self.assertAlmostEqual(self.bus.effective_speed_limits[mid],math.radians(4),places=7)
def test_higher_drive_speed_is_written_and_read_back(self):
self.bus.enable(speed_limit=math.radians(10), apply_configured_speed_limit=True)
for mid in (61,62):
self.assertAlmostEqual(self.bus.read_parameter(mid,'speed_limit'),math.radians(10),places=7)
count=len(self.peer.frames)
for invalid in [math.radians(21), float('nan'), float('inf')]:
with self.assertRaises(ValueError):
self.bus.write_parameter(61,'speed_limit',invalid)
self.assertEqual(len(self.peer.frames),count)
def test_wrong_uid_prevents_enable(self):
self.peer.motors[61]=dict(self.motors[0],uid='ff'*8)
with self.assertRaises(ValueError):self.bus.enable()
self.assertNotIn(3,[f.can_id>>24 for f in self.peer.frames])
def test_partial_enable_failure_stops_every_motor(self):
self.peer.ignore_enable.add(62)
with self.assertRaises(ValueError):self.bus.enable()
self.assertFalse(self.peer.enabled)
self.assertEqual({f.can_id&255 for f in self.peer.frames if f.can_id>>24==4},{61,62})
def test_stale_and_fault_stop(self):
self.bus.enable();self.peer.drop_feedback.add(61)
time.sleep(.65)
self.assertFalse(self.peer.enabled);self.assertIn('stale',self.bus.fault)
def test_fault_report_and_guard(self):
self.bus.enable()
self.peer.q.put(Frame((21<<24)|(61<<8)|253,struct.pack('<II',4,8)))
time.sleep(.4)
self.assertFalse(self.peer.enabled)
s=self.bus.get_state(61);self.assertEqual(s.fault_report,4);self.assertEqual(s.warning_report,8)
def test_all_targets_validated_before_any_send(self):
self.bus.enable();count=len(self.peer.frames)
with self.assertRaises(ValueError):self.bus.set_positions({61:.2,62:float('nan')})
self.assertEqual(len(self.peer.frames),count)
with self.assertRaises(ValueError):self.bus.write_parameter(61,'current_limit',99.)
def test_stop_ack_failure_is_not_success(self):
self.bus.enable();self.peer.ignore_stop.add(61)
self.assertFalse(self.bus.disable());self.assertIn(61,self.peer.enabled)
self.peer.ignore_stop.clear();self.assertTrue(self.bus.disable())
if __name__=='__main__':unittest.main()