commit 685c4d240e4abe00b4d9eef5c0685730c16fe13b Author: liyang <836021389@qq.com> Date: Tue Oct 27 13:57:39 2026 +0800 Publish standalone A7 direct CAN SDK with examples and offline tests diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e4b9b7c --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +.venv/ +__pycache__/ +*.py[cod] +*.egg-info/ +build/ +dist/ +out/ +config/local*.json diff --git a/README.md b/README.md new file mode 100644 index 0000000..78ed586 --- /dev/null +++ b/README.md @@ -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)。 diff --git a/config/left.json b/config/left.json new file mode 100644 index 0000000..9366391 --- /dev/null +++ b/config/left.json @@ -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" +} diff --git a/docs/API.md b/docs/API.md new file mode 100644 index 0000000..a789d54 --- /dev/null +++ b/docs/API.md @@ -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 的独占文件锁只能协调遵守同一锁的程序,不能阻止其他主机或不使用该锁的程序竞争总线。 diff --git a/docs/PROTOCOLS.md b/docs/PROTOCOLS.md new file mode 100644 index 0000000..8ef0dbf --- /dev/null +++ b/docs/PROTOCOLS.md @@ -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 保存、机械设零或电角度校准命令。本次上传不执行现场查询或运动。 diff --git a/docs/VALIDATION.md b/docs/VALIDATION.md new file mode 100644 index 0000000..e9c9c6d --- /dev/null +++ b/docs/VALIDATION.md @@ -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 的现场兼容性。 diff --git a/docs/inherited_source_manifest.json b/docs/inherited_source_manifest.json new file mode 100644 index 0000000..8365500 --- /dev/null +++ b/docs/inherited_source_manifest.json @@ -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" +} diff --git a/docs/package_manifest.json b/docs/package_manifest.json new file mode 100644 index 0000000..3bdba0b --- /dev/null +++ b/docs/package_manifest.json @@ -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" + } + ] +} diff --git a/docs/source_manifest.json b/docs/source_manifest.json new file mode 100644 index 0000000..52d4d36 --- /dev/null +++ b/docs/source_manifest.json @@ -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" + } + ] +} diff --git a/examples/fake_can.py b/examples/fake_can.py new file mode 100644 index 0000000..438b353 --- /dev/null +++ b/examples/fake_can.py @@ -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() diff --git a/examples/read_angles.py b/examples/read_angles.py new file mode 100644 index 0000000..e730788 --- /dev/null +++ b/examples/read_angles.py @@ -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() diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..e47524d --- /dev/null +++ b/pyproject.toml @@ -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"] diff --git a/scripts/check.sh b/scripts/check.sh new file mode 100644 index 0000000..8e848dd --- /dev/null +++ b/scripts/check.sh @@ -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 diff --git a/scripts/setup.sh b/scripts/setup.sh new file mode 100644 index 0000000..354b128 --- /dev/null +++ b/scripts/setup.sh @@ -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" diff --git a/src/a7_can_sdk/__init__.py b/src/a7_can_sdk/__init__.py new file mode 100644 index 0000000..b44f355 --- /dev/null +++ b/src/a7_can_sdk/__init__.py @@ -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'] diff --git a/src/a7_can_sdk/client.py b/src/a7_can_sdk/client.py new file mode 100644 index 0000000..9c04c92 --- /dev/null +++ b/src/a7_can_sdk/client.py @@ -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('=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 0limit: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()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 diff --git a/src/a7_can_sdk/configuration.py b/src/a7_can_sdk/configuration.py new file mode 100644 index 0000000..8cbf967 --- /dev/null +++ b/src/a7_can_sdk/configuration.py @@ -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>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('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.) diff --git a/src/a7_can_sdk/inspect_bus.py b/src/a7_can_sdk/inspect_bus.py new file mode 100644 index 0000000..68bf6b9 --- /dev/null +++ b/src/a7_can_sdk/inspect_bus.py @@ -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() diff --git a/src/a7_can_sdk/manual_client.py b/src/a7_can_sdk/manual_client.py new file mode 100644 index 0000000..95d8da5 --- /dev/null +++ b/src/a7_can_sdk/manual_client.py @@ -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=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<'):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)) diff --git a/src/a7_can_sdk/protocol.py b/src/a7_can_sdk/protocol.py new file mode 100644 index 0000000..80e6849 --- /dev/null +++ b/src/a7_can_sdk/protocol.py @@ -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('=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('>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('>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 diff --git a/src/a7_can_sdk/runtime.py b/src/a7_can_sdk/runtime.py new file mode 100644 index 0000000..a6a1041 --- /dev/null +++ b/src/a7_can_sdk/runtime.py @@ -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() diff --git a/src/a7_can_sdk/startup.py b/src/a7_can_sdk/startup.py new file mode 100644 index 0000000..d00d47e --- /dev/null +++ b/src/a7_can_sdk/startup.py @@ -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 diff --git a/src/a7_can_sdk/trace.py b/src/a7_can_sdk/trace.py new file mode 100644 index 0000000..1343a7b --- /dev/null +++ b/src/a7_can_sdk/trace.py @@ -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' diff --git a/src/a7_can_sdk/transport.py b/src/a7_can_sdk/transport.py new file mode 100644 index 0000000..e7b2c3c --- /dev/null +++ b/src/a7_can_sdk/transport.py @@ -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 diff --git a/tests/test_manual_protocol.py b/tests/test_manual_protocol.py new file mode 100644 index 0000000..5361eda --- /dev/null +++ b/tests/test_manual_protocol.py @@ -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() diff --git a/tests/test_sdk_inherited.py b/tests/test_sdk_inherited.py new file mode 100644 index 0000000..f0be63c --- /dev/null +++ b/tests/test_sdk_inherited.py @@ -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('