Publish standalone A7 direct CAN SDK with examples and offline tests
This commit is contained in:
@@ -0,0 +1,8 @@
|
||||
.venv/
|
||||
__pycache__/
|
||||
*.py[cod]
|
||||
*.egg-info/
|
||||
build/
|
||||
dist/
|
||||
out/
|
||||
config/local*.json
|
||||
@@ -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)。
|
||||
@@ -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
@@ -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 的独占文件锁只能协调遵守同一锁的程序,不能阻止其他主机或不使用该锁的程序竞争总线。
|
||||
@@ -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 保存、机械设零或电角度校准命令。本次上传不执行现场查询或运动。
|
||||
@@ -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 的现场兼容性。
|
||||
@@ -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"
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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"]
|
||||
@@ -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
|
||||
@@ -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"
|
||||
@@ -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']
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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.)
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
|
||||
@@ -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))
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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'
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
Reference in New Issue
Block a user