[software] 添加16DOF早期训练仿真与Sim2Real闭环

05_software/real/sim2real/vendored/drivers/__init__.py

@@ -0,0 +1,3 @@
+from drivers.motor_driver import RobStrideDriver, RobStrideMotor, MotorState
+from drivers.motor_params import CommunicationType, ParamIndex, RunMode
+from drivers.usb_can_adapter import DmUsbAdapter

05_software/real/sim2real/vendored/drivers/motor_driver.py

@@ -0,0 +1,371 @@
+import struct
+import time
+import queue
+import numpy as np
+from typing import Dict, Optional, Any, List
+from dataclasses import dataclass
+
+from drivers.usb_can_adapter import DmUsbAdapter
+from drivers.motor_params import (
+    CommunicationType, ParamIndex, ParamType, 
+    MODEL_MIT_POSITION_TABLE, MODEL_MIT_VELOCITY_TABLE, 
+    MODEL_MIT_TORQUE_TABLE, MODEL_MIT_KP_TABLE, MODEL_MIT_KD_TABLE,
+    get_pack_format, PARAM_TABLE
+)
+
+@dataclass
+class MotorState:
+    position: float = 0.0
+    velocity: float = 0.0
+    torque: float = 0.0
+    temperature: float = 0.0
+    current: float = 0.0
+    update_count: int = 0
+
+class RobStrideMotor:
+    def __init__(self, name: str, motor_id: int, model: str):
+        """
+        初始化电机对象。
+        
+        :param name: 电机名称 (例如 "knee")
+        :param motor_id: 电机 ID
+        :param model: 电机型号 (例如 "rs-06")
+        """
+        self.name = name
+        self.id = motor_id
+        self.model = model
+        self.state = MotorState()
+        
+    def update_state(self, pos: float, vel: float, torque: float, temp: float, current: float = 0.0):
+        """
+        更新电机状态。
+        """
+        self.state.position = pos
+        self.state.velocity = vel
+        self.state.torque = torque
+        self.state.temperature = temp
+        self.state.update_count += 1
+        if current != 0.0:
+            self.state.current = current
+
+class RobStrideDriver:
+    def __init__(self, port: str, debug: bool = False):
+        """
+        初始化驱动器。
+        
+        :param port: 串口名称
+        :param debug: 是否开启调试模式
+        """
+        self.adapter = DmUsbAdapter(port, debug=debug)
+        self.motors: Dict[str, RobStrideMotor] = {}
+        self.motors_by_id: Dict[int, RobStrideMotor] = {}
+        self.host_id = 0xFD # 根据文档，主机 ID 默认为 0xFD
+        
+        self.parameter_values = {} # 读取参数缓存: (motor_id, param_index) -> value
+        
+    def connect(self):
+        """连接到底层适配器。"""
+        self.adapter.open()
+        print(f"已连接到 RobStride 驱动器，端口: {self.adapter.serial.port}")
+        # 设置 CAN 波特率为 1000kbps (Index 0)
+        self.adapter.set_can_baudrate(0)
+        
+    def disconnect(self):
+        """断开连接。"""
+        self.adapter.close()
+        print("已断开 RobStride 驱动器连接")
+        
+    def set_can_id(self, current_id: int, new_id: int):
+        """
+        设置电机 CAN ID。
+        
+        :param current_id: 当前电机 ID
+        :param new_id: 新电机 ID
+        """
+        # Type 7: Set CAN ID
+        # Bits 23-16: New ID (Preset ID)
+        # Bits 15-8: Master ID
+        # Bits 7-0: Target ID
+        extra_data = (new_id << 8) | self.host_id
+        self._send_command(CommunicationType.SET_CAN_ID, extra_data, current_id)
+        print(f"已发送 ID 修改指令: {current_id} -> {new_id} (Master: {self.host_id})")
+
+    def scan_motors(self, timeout: float = 0.1) -> List[int]:
+        """
+        快速扫描总线上的电机 (ID 1-127)。
+        
+        :param timeout: 等待响应的超时时间
+        :return: 发现的电机 ID 列表
+        """
+        found_ids = []
+        print("正在快速扫描所有电机 (ID 1-127)...")
+        
+        # 清空缓冲区
+        while self.adapter.read_can_frame():
+            pass
+            
+        # 快速发送查询指令
+        for dev_id in range(1, 128):
+            # 发送获取设备 ID 命令
+            self._send_command(CommunicationType.GET_DEVICE_ID, self.host_id, dev_id)
+            
+        # 等待响应
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            frame = self.adapter.read_can_frame()
+            if frame:
+                can_id, data, cmd, ide, rtr = frame
+                if not ide: continue
+                
+                # 解析回复
+                # 通信类型 0 (GET_DEVICE_ID/Status)
+                comm_type = (can_id >> 24) & 0x1F
+                
+                if comm_type == CommunicationType.GET_DEVICE_ID: # Type 0
+                    # Type 0 回复格式:
+                    # Bits 23-8: Status info
+                    # Bits 7-0: Motor ID
+                    extra_data = (can_id >> 8) & 0xFFFF
+                    motor_id = extra_data & 0xFF # Device ID
+                    
+                    if motor_id not in found_ids:
+                        print(f"发现电机 ID: {motor_id}")
+                        found_ids.append(motor_id)
+                        
+        return sorted(found_ids)
+
+        
+    def add_motor(self, name: str, motor_id: int, model: str):
+        """
+        添加电机到控制列表。
+        
+        :param name: 电机名称
+        :param motor_id: 电机 ID
+        :param model: 电机型号
+        """
+        motor = RobStrideMotor(name, motor_id, model)
+        self.motors[name] = motor
+        self.motors_by_id[motor_id] = motor
+        
+    def _send_command(self, comm_type: int, extra_data: int, device_id: int, data: bytes = b''):
+        # 构建 29 位扩展 CAN ID
+        # Bits 28-24: 通信类型 (Communication Type)
+        # Bits 23-8:  额外数据 (Extra Data)
+        # Bits 7-0:   设备 ID (Device ID)
+        can_id = (comm_type << 24) | (extra_data << 8) | device_id
+        
+        # 通过适配器发送
+        # RobStride 使用扩展帧
+        self.adapter.send_can_frame(can_id, data, extended=True)
+        
+    def enable(self, motor_name: str):
+        """使能电机。"""
+        motor = self.motors[motor_name]
+        self._send_command(CommunicationType.ENABLE, self.host_id, motor.id)
+        
+    def disable(self, motor_name: str):
+        """失能电机 (Type 4: Stop)。"""
+        motor = self.motors[motor_name]
+        # Data: 全 0
+        data = bytes([0x00]*8)
+        self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
+
+    def clear_warnings(self, motor_name: str):
+        """
+        清除警告/故障 (Type 4: Stop Motor with Byte0=1)。
+        根据文档 Type 4: Byte[0]=1 时清除故障。
+        """
+        motor = self.motors[motor_name]
+        data = bytes([0x01] + [0x00]*7)
+        self._send_command(CommunicationType.DISABLE, self.host_id, motor.id, data)
+        
+    def set_zero_position(self, motor_name: str):
+        """设置电机当前位置为零点。"""
+        motor = self.motors[motor_name]
+        # Type 6: Set Zero Position
+        # Data: Byte0=1
+        data = bytes([0x01] + [0x00]*7)
+        self._send_command(CommunicationType.SET_ZERO_POSITION, self.host_id, motor.id, data)
+
+    def control_mit(self, motor_name: str, 
+                    position: float, velocity: float, 
+                    kp: float, kd: float, torque: float):
+        """
+        发送 MIT 控制指令。
+        
+        :param motor_name: 电机名称
+        :param position: 期望位置 (rad)
+        :param velocity: 期望速度 (rad/s)
+        :param kp: 位置增益
+        :param kd: 速度增益
+        :param torque: 前馈力矩 (Nm)
+        """
+        motor = self.motors[motor_name]
+        model = motor.model
+        
+        # 获取限制值
+        p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
+        v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
+        t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
+        kp_limit = MODEL_MIT_KP_TABLE.get(model, 500.0)
+        kd_limit = MODEL_MIT_KD_TABLE.get(model, 5.0)
+        
+        # 限幅
+        position = np.clip(position, -p_limit, p_limit)
+        velocity = np.clip(velocity, -v_limit, v_limit)
+        kp = np.clip(kp, 0, kp_limit)
+        kd = np.clip(kd, 0, kd_limit)
+        torque = np.clip(torque, -t_limit, t_limit)
+        
+        # 转换为 uint16
+        # Position: [-L, L] -> [0, 65535]
+        p_u16 = int(((position / p_limit) + 1.0) * 32767.0)
+        p_u16 = np.clip(p_u16, 0, 65535)
+        
+        # Velocity: [-L, L] -> [0, 65535]
+        v_u16 = int(((velocity / v_limit) + 1.0) * 32767.0)
+        v_u16 = np.clip(v_u16, 0, 65535)
+        
+        # Kp: [0, L] -> [0, 65535]
+        kp_u16 = int((kp / kp_limit) * 65535.0)
+        kp_u16 = np.clip(kp_u16, 0, 65535)
+        
+        # Kd: [0, L] -> [0, 65535]
+        kd_u16 = int((kd / kd_limit) * 65535.0)
+        kd_u16 = np.clip(kd_u16, 0, 65535)
+        
+        # Torque: [-L, L] -> [0, 65535] (发送在 Extra Data 域)
+        t_u16 = int(((torque / t_limit) + 1.0) * 32767.0)
+        t_u16 = np.clip(t_u16, 0, 65535)
+        
+        # 打包数据 (大端序)
+        data = struct.pack('>HHHH', p_u16, v_u16, kp_u16, kd_u16)
+        
+        # 发送
+        self._send_command(CommunicationType.OPERATION_CONTROL, t_u16, motor.id, data)
+
+    def read_parameter(self, motor_id: int, param_index: int):
+        """
+        发送读取参数指令 (Type 17)。
+        """
+        # Type 17
+        # Data: Index (2B) + 00 00 + 00 00 00 00
+        data = struct.pack('<H', param_index) + b'\x00\x00\x00\x00\x00\x00'
+        self._send_command(CommunicationType.READ_PARAMETER, self.host_id, motor_id, data)
+        
+    def write_parameter(self, motor_id: int, param_index: int, value: Any):
+        """
+        发送写入参数指令 (Type 18)。
+        """
+        param_info = PARAM_TABLE.get(param_index)
+        if not param_info:
+            print(f"未知参数索引: {param_index}")
+            return
+            
+        # motor_params.py format: (name, p_type, size)
+        name, p_type, size = param_info
+        
+        fmt, _ = get_pack_format(p_type)
+        if not fmt:
+            print(f"不支持的参数类型: {p_type}")
+            return
+
+        # 注意：不再进行范围检查，因为 motor_params.py 中没有定义范围
+        
+        # 打包数据
+        val_bytes = struct.pack(fmt, value)
+        # 填充 val_bytes 到 4 字节
+        if len(val_bytes) < 4:
+            val_bytes += b'\x00' * (4 - len(val_bytes))
+            
+        # Index (2B) + 00 00 + Value (4B)
+        data = struct.pack('<H', param_index) + b'\x00\x00' + val_bytes
+            
+        self._send_command(CommunicationType.WRITE_PARAMETER, self.host_id, motor_id, data)
+
+    def save_parameters(self, motor_id: int):
+        """
+        保存参数到 EEPROM (Type 22)。
+        """
+        data = bytes([0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08])
+        self._send_command(CommunicationType.SAVE_PARAMETERS, self.host_id, motor_id, data)
+
+    def process_messages(self, max_messages=50):
+        """
+        从 CAN 总线读取消息并更新电机状态。
+        """
+        count = 0
+        while count < max_messages:
+            frame = self.adapter.read_can_frame()
+            if not frame:
+                break
+                
+            can_id, data, cmd, ide, rtr = frame
+            
+            if not ide: 
+                continue # 跳过标准帧
+                
+            # 解析扩展 ID
+            comm_type = (can_id >> 24) & 0x1F
+            
+            if comm_type == CommunicationType.READ_PARAMETER:
+                # 解析参数读取反馈 (Type 17)
+                extra_data = (can_id >> 8) & 0xFFFF
+                success_flag = (extra_data >> 8) & 0xFF
+                motor_id = extra_data & 0xFF
+                
+                if success_flag == 0: # 0 表示成功
+                    if len(data) >= 8:
+                        param_index = struct.unpack('<H', data[0:2])[0]
+                        raw_value = data[4:8]
+                        
+                        param_info = PARAM_TABLE.get(param_index)
+                        if param_info:
+                            name, p_type, size = param_info
+                            fmt, _ = get_pack_format(p_type)
+                            if fmt:
+                                try:
+                                    # 根据类型大小解包
+                                    val_size = struct.calcsize(fmt)
+                                    val = struct.unpack(fmt, raw_value[:val_size])[0]
+                                    self.parameter_values[(motor_id, param_index)] = val
+                                    
+                                    # 如果是 IQF (电流)，更新电机状态
+                                    if param_index == ParamIndex.IQF:
+                                        if motor_id in self.motors_by_id:
+                                            self.motors_by_id[motor_id].state.current = val
+                                            
+                                except Exception as e:
+                                    print(f"解析参数失败: {e}")
+                else:
+                    print(f"读取参数失败，错误码: {success_flag}")
+
+            elif comm_type == CommunicationType.OPERATION_STATUS:
+                # 处理电机反馈
+                extra_data = (can_id >> 8) & 0xFFFF
+                motor_id = extra_data & 0xFF
+                if motor_id in self.motors_by_id:
+                    motor = self.motors_by_id[motor_id]
+                    self._parse_feedback(motor, data)
+            
+            count += 1
+
+    def _parse_feedback(self, motor: RobStrideMotor, data: bytes):
+        if len(data) < 8:
+            return
+            
+        # 解包大端序数据
+        p_u16, v_u16, t_i16, temp_u16 = struct.unpack('>HHHH', data)
+        
+        model = motor.model
+        p_limit = MODEL_MIT_POSITION_TABLE.get(model, 12.5)
+        v_limit = MODEL_MIT_VELOCITY_TABLE.get(model, 50.0)
+        t_limit = MODEL_MIT_TORQUE_TABLE.get(model, 60.0)
+        
+        # 转换回浮点数
+        pos = (float(p_u16) / 32767.0 - 1.0) * p_limit
+        vel = (float(v_u16) / 32767.0 - 1.0) * v_limit
+        torque = (float(t_i16) / 32767.0 - 1.0) * t_limit
+        temp = float(temp_u16) * 0.1
+        
+        motor.update_state(pos, vel, torque, temp)

05_software/real/sim2real/vendored/drivers/motor_params.py

@@ -0,0 +1,422 @@
+import numpy as np
+import struct
+
+class CommunicationType:
+    """
+    电机通信类型定义 (Bit28~24)
+    参考说明书 4.1 章节
+    
+    通信 ID 结构 (29位扩展帧):
+    | Bit 28-24 | Bit 23-8 | Bit 7-0 |
+    | 通信类型   | 数据区2  | 目标地址 |
+    """
+    GET_DEVICE_ID       = 0   # 获取设备 ID 和 64 位 MCU 唯一标识符 (Type 0)
+    OPERATION_CONTROL   = 1   # 运控模式电机控制指令 (MIT 模式) (Type 1)
+    OPERATION_STATUS    = 2   # 电机反馈数据 (标准反馈帧) (Type 2)
+    ENABLE              = 3   # 电机使能运行 (Type 3)
+    DISABLE             = 4   # 电机停止运行 (可用于清除故障) (Type 4)
+    SET_ZERO_POSITION   = 6   # 设置电机机械零位 (设置当前位置为零点) (Type 6)
+    SET_CAN_ID          = 7   # 设置电机 CAN ID (立即生效，需保存) (Type 7)
+    READ_PARAMETER      = 17  # 单个参数读取 (Type 17, 0x11)
+    WRITE_PARAMETER     = 18  # 单个参数写入 (Type 18, 0x12, 掉电丢失)
+    FAULT_REPORT        = 21  # 故障反馈帧 (Type 21, 0x15)
+    SAVE_PARAMETERS     = 22  # 电机数据保存帧 (保存所有参数到 Flash) (Type 22)
+    SET_BAUDRATE        = 23  # 电机波特率修改帧 (重新上电生效) (Type 23)
+    ACTIVE_REPORT       = 24  # 电机主动上报设置帧 (开启/关闭主动上报) (Type 24)
+    PROTOCOL_SWITCH     = 25  # 电机协议修改帧 (切换 Canopen/MIT/私有协议) (Type 25)
+    READ_VERSION        = 26  # 版本号读取帧 (Type 26)
+
+class RunMode:
+    """
+    电机运行模式 (参数索引 0x7005)
+    参考说明书 4.3 章节
+    """
+    MIT                 = 0   # 运控模式 (默认): 适用于高动态响应控制
+    POS_PP              = 1   # 位置模式 (PP): 梯形加减速位置控制
+    SPEED               = 2   # 速度模式: 闭环速度控制
+    CURRENT             = 3   # 电流模式: 闭环力矩(电流)控制
+    POS_CSP             = 5   # 位置模式 (CSP): 循环同步位置模式 (适用于周期性指令)
+
+class BaudRate:
+    """
+    电机波特率 (通信类型 23)
+    参考说明书 4.1 通信类型 23
+    注意: 修改后需重新上电生效
+    """
+    BAUD_1M             = 1   # 1 Mbps (默认)
+    BAUD_500K           = 2   # 500 Kbps
+    BAUD_250K           = 3   # 250 Kbps
+    BAUD_125K           = 4   # 125 Kbps
+
+class ActiveReportStatus:
+    """
+    电机主动上报状态 (通信类型 24)
+    参考说明书 4.1 通信类型 24
+    """
+    DISABLE             = 0   # 关闭主动上报 (默认)
+    ENABLE              = 1   # 开启主动上报 (默认间隔 10ms, 可通过 EP_SCAN_TIME 修改)
+
+class ProtocolType:
+    """
+    电机协议类型 (通信类型 25)
+    参考说明书 4.2.4 章节
+    注意: 切换协议后需重新上电生效
+    """
+    PRIVATE             = 0   # 私有协议 (默认): 使用 29 位扩展帧
+    CANOPEN             = 1   # CANopen 协议: 符合 CiA 402 标准
+    MIT                 = 2   # MIT 协议 (标准帧): 使用 11 位标准帧
+
+class ParamType:
+    """
+    参数数据类型定义
+    
+    - 私有协议 (Type 17/18) 参数表主要使用 UINT8/UINT16/UINT32/FLOAT
+    - CANopen 对象字典会用到有符号类型 (INTEGER8/16/32)
+    """
+    UINT8               = 0   # 无符号 8 位整数
+    UINT16              = 1   # 无符号 16 位整数
+    UINT32              = 2   # 无符号 32 位整数
+    FLOAT               = 3   # 32 位浮点数 (IEEE 754)
+    INT8                = 4   # 有符号 8 位整数
+    INT16               = 5   # 有符号 16 位整数
+    INT32               = 6   # 有符号 32 位整数
+
+class ErrorCode:
+    """
+    异常状态 fault 值位定义
+    
+    说明书位置:
+    - 章节 6 (Mit) 的“异常状态应答帧”对 fault 值 bit 位做了明确描述
+    - 私有协议 Type 21 故障反馈帧也会携带 fault/warning 值
+    """
+    OVER_TEMP           = 1 << 0  # bit0: 电机过温故障 (默认 >145°C)
+    DRIVE_CHIP          = 1 << 1  # bit1: 驱动芯片故障 (DRV8353 等报告错误)
+    UNDER_VOLTAGE       = 1 << 2  # bit2: 欠压故障 (电压 < 12V)
+    OVER_VOLTAGE        = 1 << 3  # bit3: 过压故障 (电压 > 60V)
+    CURRENT_B_OVER      = 1 << 4  # bit4: B 相电流采样过流
+    CURRENT_C_OVER      = 1 << 5  # bit5: C 相电流采样过流
+    ENCODER_NOT_CALIB   = 1 << 7  # bit7: 编码器未标定
+    HARDWARE_ERR        = 1 << 8  # bit8: 硬件识别故障
+    POS_INIT_ERR        = 1 << 9  # bit9: 位置初始化故障
+    LOAD_BLOCK          = 1 << 14 # bit14: 堵转过载算法保护
+    CURRENT_A_OVER      = 1 << 16 # bit16: A 相电流采样过流
+
+class WarningCode:
+    """
+    预警状态 warning 值位定义 (Type 21 Byte 4-7)
+    """
+    OVER_TEMP_WARNING   = 1 << 0  # bit0: 电机过温预警 (默认 >135°C)
+
+class DriveFault1:
+    """
+    驱动芯片故障码 1 (0x3024) - DRV8353 状态寄存器 1
+    参考说明书 3.3.7 章节
+    """
+    VDS_LC              = 1 << 0  # VDS overcurrent on C low-side (C相下管VDS过流)
+    VDS_HC              = 1 << 1  # VDS overcurrent on C high-side (C相上管VDS过流)
+    VDS_LB              = 1 << 2  # VDS overcurrent on B low-side (B相下管VDS过流)
+    VDS_HB              = 1 << 3  # VDS overcurrent on B high-side (B相上管VDS过流)
+    VDS_LA              = 1 << 4  # VDS overcurrent on A low-side (A相下管VDS过流)
+    VDS_HA              = 1 << 5  # VDS overcurrent on A high-side (A相上管VDS过流)
+    OTSD                = 1 << 6  # Overtemperature shutdown (过温关断)
+    UVLO                = 1 << 7  # Undervoltage lockout (欠压锁定)
+    GDF                 = 1 << 8  # Gate drive fault (栅极驱动故障)
+    VDS_OCP             = 1 << 9  # VDS monitor overcurrent (VDS 监控过流)
+    FAULT               = 1 << 10 # Logic OR of FAULT status (故障状态逻辑或)
+
+class DriveFault2:
+    """
+    驱动芯片故障码 2 (0x3025) - DRV8353 状态寄存器 2
+    参考说明书 3.3.7 章节
+    """
+    VGS_LC              = 1 << 0  # Gate drive fault on C low-side (C相下管栅极故障)
+    VGS_HC              = 1 << 1  # Gate drive fault on C high-side (C相上管栅极故障)
+    VGS_LB              = 1 << 2  # Gate drive fault on B low-side (B相下管栅极故障)
+    VGS_HB              = 1 << 3  # Gate drive fault on B high-side (B相上管栅极故障)
+    VGS_LA              = 1 << 4  # Gate drive fault on A low-side (A相下管栅极故障)
+    VGS_HA              = 1 << 5  # Gate drive fault on A high-side (A相上管栅极故障)
+    GDUV                = 1 << 6  # VCP charge pump / VGLS undervoltage (电荷泵欠压)
+    OTW                 = 1 << 7  # Overtemperature warning (过温预警)
+    SC_OC               = 1 << 8  # Overcurrent on phase C sense amplifier (C相采样过流)
+    SB_OC               = 1 << 9  # Overcurrent on phase B sense amplifier (B相采样过流)
+    SA_OC               = 1 << 10 # Overcurrent on phase A sense amplifier (A相采样过流)
+
+class MotorParams:
+    """
+    电机物理参数限制 (用于 MIT 模式数据压缩)
+    参考说明书 4.1 通信类型 1
+    
+    注意:
+    - P_MIN/MAX: 位置范围 (RS03: -12.57 ~ 12.57 rad)
+    - V_MIN/MAX: 速度范围 (RS03: -20 ~ 20 rad/s)
+    - T_MIN/MAX: 力矩范围 (RS03: -60 ~ 60 Nm)
+    - KP/KD: 刚度和阻尼系数范围
+    """
+    def __init__(self,
+                 p_min: float = -12.57,
+                 p_max: float = 12.57,  # RS03: -12.57 ~ 12.57 rad (约 -4pi ~ 4pi)
+                 v_min: float = -20.0,
+                 v_max: float = 20.0,   # RS03: -20 ~ 20 rad/s
+                 kp_min: float = 0.0,
+                 kp_max: float = 5000.0, # RS03: 0 ~ 5000
+                 kd_min: float = 0.0,
+                 kd_max: float = 100.0,  # RS03: 0 ~ 100
+                 t_min: float = -60.0,
+                 t_max: float = 60.0):  # RS03: -60 ~ 60 Nm
+        self.P_MIN = p_min
+        self.P_MAX = p_max
+        self.V_MIN = v_min
+        self.V_MAX = v_max
+        self.KP_MIN = kp_min
+        self.KP_MAX = kp_max
+        self.KD_MIN = kd_min
+        self.KD_MAX = kd_max
+        self.T_MIN = t_min
+        self.T_MAX = t_max
+
+class ParamIndex:
+    """
+    电机参数索引表 (Index)
+    参考说明书 4.1 可读写单个参数列表
+    """
+    RUN_MODE            = 0x7005  # 运行模式: 0:运控, 1:PP, 2:速度, 3:电流, 5:CSP (W/R)
+    IQ_REF              = 0x7006  # 电流模式 Iq 指令 (-43~43A) (W/R)
+    SPD_REF             = 0x700A  # 转速模式转速指令 (-20~20rad/s) (W/R)
+    LIMIT_TORQUE        = 0x700B  # 转矩限制 (0~60Nm) (W/R)
+    CUR_KP              = 0x7010  # 电流 Kp (默认 0.17) (W/R)
+    CUR_KI              = 0x7011  # 电流 Ki (默认 0.012) (W/R)
+    CUR_FILT_GAIN       = 0x7014  # 电流滤波系数 (0~1.0, 默认 0.1) (W/R)
+    LOC_REF             = 0x7016  # 位置模式角度指令 (rad) (W/R)
+    LIMIT_SPD           = 0x7017  # 位置模式(CSP)速度限制 (0~20rad/s) (W/R)
+    LIMIT_CUR           = 0x7018  # 速度/位置模式电流限制 (0~43A) (W/R)
+    MECH_POS            = 0x7019  # 负载端计圈机械角度 (rad) (Read Only)
+    IQF                 = 0x701A  # Iq 滤波值 (A) (Read Only)
+    MECH_VEL            = 0x701B  # 负载端转速 (rad/s) (Read Only)
+    VBUS                = 0x701C  # 母线电压 (V) (Read Only)
+    LOC_KP              = 0x701E  # 位置环 Kp (默认 60) (W/R)
+    SPD_KP              = 0x701F  # 速度环 Kp (默认 6) (W/R)
+    SPD_KI              = 0x7020  # 速度环 Ki (默认 0.02) (W/R)
+    SPD_FILT_GAIN       = 0x7021  # 速度滤波值 (默认 0.1) (W/R)
+    ACC_RAD             = 0x7022  # 速度模式加速度 (默认 20rad/s^2) (W/R)
+    VEL_MAX             = 0x7024  # 位置模式(PP)速度 (默认 10rad/s) (W/R)
+    ACC_SET             = 0x7025  # 位置模式(PP)加速度 (默认 10rad/s^2) (W/R)
+    EP_SCAN_TIME        = 0x7026  # 主动上报时间 (1=10ms, +1=+5ms) (W)
+    CAN_TIMEOUT         = 0x7028  # CAN 超时阈值 (20000=1s, 0=禁用) (W)
+    ZERO_STA            = 0x7029  # 零点标志位 (0: 0~2pi, 1: -pi~pi) (W)
+    DAMPER              = 0x702A  # 阻尼开关 (1: 取消关机反驱保护) (W/R)
+    ADD_OFFSET          = 0x702B  # 零位偏置 (rad) (W/R)
+
+class CanopenIndex:
+    """
+    CANopen 对象字典常用索引
+    参考说明书第 5 章 (Canopen)
+    """
+    ERROR_CODE                   = 0x603F  # 错误码
+    CONTROLWORD                  = 0x6040  # 控制字
+    STATUSWORD                   = 0x6041  # 状态字
+    MODES_OF_OPERATION           = 0x6060  # 运行模式
+    MODES_OF_OPERATION_DISPLAY   = 0x6061  # 当前运行模式显示
+    POSITION_DEMAND_VALUE        = 0x6062  # 位置指令值
+    POSITION_ACTUAL_VALUE        = 0x6064  # 位置实际值
+    POSITION_WINDOW              = 0x6067  # 位置窗口
+    POSITION_WINDOW_TIME         = 0x6068  # 位置窗口时间
+    VELOCITY_DEMAND_VALUE        = 0x606B  # 速度指令值
+    VELOCITY_ACTUAL_VALUE        = 0x606C  # 速度实际值
+    TARGET_TORQUE                = 0x6071  # 目标力矩 (0.1% 额定力矩)
+    TORQUE_ACTUAL_VALUE          = 0x6077  # 力矩实际值
+    CURRENT_ACTUAL_VALUE         = 0x6078  # 电流实际值
+    DC_LINK_CIRCUIT_VOLTAGE      = 0x6079  # 母线电压
+    TARGET_POSITION              = 0x607A  # 目标位置
+    PROFILE_VELOCITY             = 0x6081  # 轮廓速度
+    PROFILE_ACCELERATION         = 0x6083  # 轮廓加速度
+    TARGET_VELOCITY              = 0x60FF  # 目标速度
+
+class CanopenModeOfOperation:
+    """CANopen 模式 (6060)"""
+    PP       = 1  # Profile Position Mode
+    SPEED    = 3  # Profile Velocity Mode
+    TORQUE   = 4  # Profile Torque Mode
+    CSP      = 5  # Cyclic Synchronous Position Mode
+    HOMING   = 6  # Homing Mode
+
+class CanopenControlword:
+    """CANopen 控制字 (6040) 常用值"""
+    SHUTDOWN           = 0x0006  # Shutdown
+    SWITCH_ON          = 0x0007  # Switch On
+    ENABLE_OPERATION   = 0x000F  # Enable Operation
+    DISABLE_VOLTAGE    = 0x0001  # Disable Voltage
+    QUICK_STOP         = 0x000B  # Quick Stop
+
+# CANopen 协议切换帧 (扩展帧)
+# 说明书 5.10: 29 位 ID 为 0xFFF，数据区 Byte0~6 固定 01~06，Byte7=F_CMD(协议类型)
+CANOPEN_PROTOCOL_SWITCH_EXT_ID = 0xFFF
+
+class MitStdCommandType:
+    """
+    MIT 标准帧指令类型 (对应说明书第 6 章的指令 1~11)
+    
+    标准帧 ID (11位) 结构:
+    | Bit 10-8 | Bit 7-0 |
+    | 模式/指令 | 电机 ID |
+    
+    注意:
+    - 指令 1~9: CAN ID 的 Bit10~8 为 0，通过数据区 Payload 区分功能
+    - 指令 10:  CAN ID 的 Bit10~8 为 1 (位置模式)
+    - 指令 11:  CAN ID 的 Bit10~8 为 2 (速度模式)
+    """
+    ENABLE                      = 1   # 指令 1: 电机使能运行
+    STOP                        = 2   # 指令 2: 电机停止运行
+    DYNAMIC_PARAM               = 3   # 指令 3: MIT 动态参数
+    SET_ZERO                    = 4   # 指令 4: 设置零点 (非位置模式)
+    CLEAR_ERROR_OR_READ_STATUS  = 5   # 指令 5: 清错 / 读取异常状态
+    SET_RUN_MODE                = 6   # 指令 6: 设置运行模式
+    SET_MOTOR_CAN_ID            = 7   # 指令 7: 修改电机 CANID
+    SET_PROTOCOL                = 8   # 指令 8: 修改电机协议 (重新上电生效)
+    SET_MASTER_CAN_ID           = 9   # 指令 9: 修改主机 CANID
+    POS_CONTROL                 = 10  # 指令 10: 位置模式控制指令 (ID Bit10-8=1)
+    SPEED_CONTROL               = 11  # 指令 11: 速度模式控制指令 (ID Bit10-8=2)
+
+def get_mit_can_id_mode(cmd_type: int) -> int:
+    """
+    获取 MIT 标准帧 CAN ID 的 Bit10~8 值
+    
+    :param cmd_type: MitStdCommandType 枚举值
+    :return: 模式位 (0, 1, 或 2)
+    """
+    if cmd_type in (MitStdCommandType.POS_CONTROL,):
+        return 1
+    elif cmd_type in (MitStdCommandType.SPEED_CONTROL,):
+        return 2
+    else:
+        # 指令 1~9 (以及其他潜在指令) 默认为 0
+        return 0
+
+def build_mit_std_id(cmd_type: int, motor_id: int) -> int:
+    """
+    构建 MIT 标准帧 11 位 CAN ID
+    
+    :param cmd_type: MitStdCommandType 枚举值
+    :param motor_id: 电机 ID (0~127)
+    :return: 11 位 CAN ID
+    """
+    mode = get_mit_can_id_mode(cmd_type)
+    return ((mode & 0x07) << 8) | (motor_id & 0xFF)
+
+class MitPayloads:
+    """
+    MIT 协议特殊指令的固定 Payload 定义 (指令 1, 2, 4, 5, 6, 7, 8, 9)
+    部分指令的 Payload 末尾字节需要根据参数动态修改
+    """
+    # 指令 1: FF FF FF FF FF FF FF FC
+    ENABLE = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFC'
+    
+    # 指令 2: FF FF FF FF FF FF FF FD
+    STOP = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFD'
+    
+    # 指令 3: 动态参数 (全 0 或根据参数设置)
+    DYNAMIC_PARAM_ZERO = b'\x00\x00\x00\x00\x00\x00\x00\x00'
+    
+    # 指令 4: FF FF FF FF FF FF FF FE
+    SET_ZERO = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFE'
+    
+    # 指令 5: FF FF FF FF FF FF FF FB (清除错误)
+    # 若 F_CMD (Byte6) 为 0xFF 则清除错误，否则为读取异常状态
+    CLEAR_ERROR = b'\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFB'
+    
+    # 指令 6: FF FF FF FF FF FF [Mode] FC
+    # Template, last 2 bytes are [Mode, FC]
+    SET_RUN_MODE_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 7: FF FF FF FF FF FF [NewID] FA
+    SET_MOTOR_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 8: FF FF FF FF FF FF [Protocol] FD
+    SET_PROTOCOL_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+    
+    # 指令 9: FF FF FF FF FF FF [MasterID] 01
+    SET_MASTER_CAN_ID_PREFIX = b'\xFF\xFF\xFF\xFF\xFF\xFF'
+
+
+# 参数表配置: (参数名, 数据类型, 字节数)
+PARAM_TABLE = {
+    ParamIndex.RUN_MODE:      ("run_mode", ParamType.UINT8, 1),
+    ParamIndex.IQ_REF:        ("iq_ref", ParamType.FLOAT, 4),
+    ParamIndex.SPD_REF:       ("spd_ref", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_TORQUE:  ("limit_torque", ParamType.FLOAT, 4),
+    ParamIndex.CUR_KP:        ("cur_kp", ParamType.FLOAT, 4),
+    ParamIndex.CUR_KI:        ("cur_ki", ParamType.FLOAT, 4),
+    ParamIndex.CUR_FILT_GAIN: ("cur_filt_gain", ParamType.FLOAT, 4),
+    ParamIndex.LOC_REF:       ("loc_ref", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_SPD:     ("limit_spd", ParamType.FLOAT, 4),
+    ParamIndex.LIMIT_CUR:     ("limit_cur", ParamType.FLOAT, 4),
+    ParamIndex.MECH_POS:      ("mechPos", ParamType.FLOAT, 4),
+    ParamIndex.IQF:           ("iqf", ParamType.FLOAT, 4),
+    ParamIndex.MECH_VEL:      ("mechVel", ParamType.FLOAT, 4),
+    ParamIndex.VBUS:          ("VBUS", ParamType.FLOAT, 4),
+    ParamIndex.LOC_KP:        ("loc_kp", ParamType.FLOAT, 4),
+    ParamIndex.SPD_KP:        ("spd_kp", ParamType.FLOAT, 4),
+    ParamIndex.SPD_KI:        ("spd_ki", ParamType.FLOAT, 4),
+    ParamIndex.SPD_FILT_GAIN: ("spd_filt_gain", ParamType.FLOAT, 4),
+    ParamIndex.ACC_RAD:       ("acc_rad", ParamType.FLOAT, 4),
+    ParamIndex.VEL_MAX:       ("vel_max", ParamType.FLOAT, 4),
+    ParamIndex.ACC_SET:       ("acc_set", ParamType.FLOAT, 4),
+    ParamIndex.EP_SCAN_TIME:  ("EPScan_time", ParamType.UINT16, 2),
+    ParamIndex.CAN_TIMEOUT:   ("cantimeout", ParamType.UINT32, 4),
+    ParamIndex.ZERO_STA:      ("zero_sta", ParamType.UINT8, 1),
+    ParamIndex.DAMPER:        ("damper", ParamType.UINT8, 1),
+    ParamIndex.ADD_OFFSET:    ("add_offset", ParamType.FLOAT, 4),
+}
+
+MODEL_MIT_POSITION_TABLE = {
+    "rs-00": 4 * np.pi, "rs-01": 4 * np.pi, "rs-02": 4 * np.pi,
+    "rs-03": 4 * np.pi, "rs-04": 4 * np.pi, "rs-05": 4 * np.pi, "rs-06": 4 * np.pi,
+    "el-05": 4 * np.pi,
+}
+
+MODEL_MIT_VELOCITY_TABLE = {
+    "rs-00": 50, "rs-01": 44, "rs-02": 44,
+    "rs-03": 50, "rs-04": 15, "rs-05": 33, "rs-06": 20,
+    "el-05": 50,
+}
+
+MODEL_MIT_TORQUE_TABLE = {
+    "rs-00": 17, "rs-01": 17, "rs-02": 17,
+    "rs-03": 60, "rs-04": 120, "rs-05": 17, "rs-06": 60,
+    "el-05": 6,
+}
+
+MODEL_MIT_KP_TABLE = {
+    "rs-00": 500.0, "rs-01": 500.0, "rs-02": 500.0,
+    "rs-03": 5000.0, "rs-04": 5000.0, "rs-05": 500.0, "rs-06": 5000.0,
+    "el-05": 500.0,
+}
+
+MODEL_MIT_KD_TABLE = {
+    "rs-00": 5.0, "rs-01": 5.0, "rs-02": 5.0,
+    "rs-03": 100.0, "rs-04": 100.0, "rs-05": 5.0, "rs-06": 100.0,
+    "el-05": 5.0,
+}
+
+def get_pack_format(param_type):
+    """
+    获取 struct.pack 的格式字符串和字节大小
+
+    说明:
+    - Type 17/18 参数读写使用小端序
+    - CANopen SDO 数据同样通常按小端序解释 (取决于实现)
+    """
+    if param_type == ParamType.UINT8:
+        return '<B', 1
+    elif param_type == ParamType.UINT16:
+        return '<H', 2
+    elif param_type == ParamType.UINT32:
+        return '<I', 4
+    elif param_type == ParamType.INT8:
+        return '<b', 1
+    elif param_type == ParamType.INT16:
+        return '<h', 2
+    elif param_type == ParamType.INT32:
+        return '<i', 4
+    elif param_type == ParamType.FLOAT:
+        return '<f', 4
+    return None, 0

05_software/real/sim2real/vendored/drivers/usb_can_adapter.py

@@ -0,0 +1,185 @@
+import serial
+import struct
+import time
+from typing import Optional, Tuple
+
+class DmUsbAdapter:
+    """
+    达妙 USB 转 CAN 适配器驱动。
+    处理底层串口通信和帧的封装/解包。
+    """
+    
+    # 帧常量
+    SEND_HEADER = b'\x55\xAA'
+    SEND_FRAME_LEN = 30
+    RECV_HEADER = 0xAA
+    RECV_TAIL = 0x55
+    RECV_FRAME_LEN = 16
+    
+    def __init__(self, port: str, baudrate: int = 921600, timeout: float = 0.01, debug: bool = False):
+        """
+        初始化 USB 转 CAN 适配器。
+        
+        :param port: 串口名称 (例如 "COM3")
+        :param baudrate: 串口波特率 (默认 921600)
+        :param timeout: 读取超时时间 (秒)
+        :param debug: 是否打印调试信息
+        """
+        self.serial = serial.Serial()
+        self.serial.port = port
+        self.serial.baudrate = baudrate
+        self.serial.timeout = timeout
+        self.data_buffer = bytearray()
+        self.debug = debug
+
+    def open(self):
+        """打开串口连接。"""
+        if not self.serial.is_open:
+            try:
+                self.serial.open()
+                if self.debug:
+                    print(f"[DEBUG] 串口 {self.serial.port} 已打开")
+            except Exception as e:
+                print(f"[ERROR] 无法打开串口 {self.serial.port}: {e}")
+                raise
+
+    def close(self):
+        """关闭串口连接。"""
+        if self.serial.is_open:
+            self.serial.close()
+            if self.debug:
+                print(f"[DEBUG] 串口 {self.serial.port} 已关闭")
+
+    def set_can_baudrate(self, index: int = 0):
+        """
+        设置 CAN 波特率。
+        
+        索引对照表:
+        0: 1000 kbps
+        1: 800 kbps
+        2: 666 kbps
+        3: 500 kbps
+        ...
+        63: 
+        
+        :param index: 波特率索引 (默认 0, 即 1000kbps)
+        """
+        # 构建设置波特率指令: 55 05 Index(1byte) AA 55
+        cmd = bytearray([0x55, 0x05, index & 0xFF, 0xAA, 0x55])
+        self.serial.write(cmd)
+        if self.debug:
+            print(f"[DEBUG] 发送设置波特率指令: {cmd.hex()}")
+        time.sleep(0.1) # 等待生效
+
+    def send_can_frame(self, can_id: int, data: bytes, 
+                       extended: bool = True, remote: bool = False, 
+                       feedback: bool = False) -> None:
+        """
+        发送 CAN 帧。
+        
+        :param can_id: CAN 标识符 (标准帧或扩展帧)
+        :param data: 数据负载 (最多 8 字节)
+        :param extended: True 为扩展帧 (29位), False 为标准帧 (11位)
+        :param remote: True 为远程帧, False 为数据帧
+        :param feedback: True 请求设备反馈 (CMD 0x01), False 不反馈 (CMD 0x03)
+        """
+        if len(data) > 8:
+            raise ValueError("CAN 数据不能超过 8 字节")
+            
+        # 填充数据到 8 字节
+        data_padded = data + b'\x00' * (8 - len(data))
+        
+        cmd = 0x01 if feedback else 0x03
+        send_count = 1
+        interval = 10 # 默认 10ms
+        id_type = 1 if extended else 0
+        frame_type = 1 if remote else 0
+        data_len = len(data)
+        
+        # 构建帧 (30 字节)
+        frame = bytearray(30)
+        frame[0] = 0x55
+        frame[1] = 0xAA
+        frame[2] = 0x1E # 长度
+        frame[3] = cmd
+        
+        # 发送次数 (4 字节, 小端序)
+        frame[4:8] = struct.pack('<I', send_count)
+        
+        # 时间间隔 (4 字节, 小端序)
+        frame[8:12] = struct.pack('<I', interval)
+        
+        frame[12] = id_type
+        
+        # CAN ID (4 字节, 小端序)
+        frame[13:17] = struct.pack('<I', can_id)
+        
+        frame[17] = frame_type
+        frame[18] = data_len
+        # 19, 20 为保留位 0
+        
+        frame[21:29] = data_padded
+        frame[29] = 0x00 # CRC (任意值)
+        
+        self.serial.write(frame)
+        
+        if self.debug:
+            print(f"[DEBUG] 发送帧: ID=0x{can_id:08X} Data={data.hex()} Raw={frame.hex()}")
+
+    def read_can_frame(self) -> Optional[Tuple[int, bytes, int, bool, bool]]:
+        """
+        如果缓冲区中有可用数据，读取一帧 CAN 数据。
+        
+        :return: 元组 (can_id, data, cmd, extended, remote) 或者 None (如果没有完整帧)
+        """
+        # 读取可用数据
+        if self.serial.in_waiting:
+            raw_data = self.serial.read(self.serial.in_waiting)
+            self.data_buffer.extend(raw_data)
+            
+        # 检查完整帧 (16 字节)
+        while len(self.data_buffer) >= self.RECV_FRAME_LEN:
+            # 查找帧头
+            try:
+                header_idx = self.data_buffer.index(self.RECV_HEADER)
+            except ValueError:
+                # 没有找到帧头，清空缓冲区（保留最后几个字节以防截断）
+                self.data_buffer = self.data_buffer[-(self.RECV_FRAME_LEN-1):]
+                return None
+                
+            # 检查从帧头开始是否有足够字节
+            if len(self.data_buffer) - header_idx < self.RECV_FRAME_LEN:
+                # 保留从帧头开始的数据
+                self.data_buffer = self.data_buffer[header_idx:]
+                return None
+                
+            # 检查帧尾
+            if self.data_buffer[header_idx + self.RECV_FRAME_LEN - 1] != self.RECV_TAIL:
+                # 无效帧，跳过该帧头继续查找
+                self.data_buffer = self.data_buffer[header_idx + 1:]
+                continue
+                
+            # 提取有效帧
+            frame = self.data_buffer[header_idx : header_idx + self.RECV_FRAME_LEN]
+            self.data_buffer = self.data_buffer[header_idx + self.RECV_FRAME_LEN:]
+            
+            if self.debug:
+                print(f"[DEBUG] 解析帧: {frame.hex()}")
+
+            # 解析帧
+            cmd = frame[1]
+            format_byte = frame[2]
+            
+            data_len = format_byte & 0x3F
+            ide = bool((format_byte >> 6) & 0x01)
+            rtr = bool((format_byte >> 7) & 0x01)
+            
+            can_id = struct.unpack('<I', frame[3:7])[0]
+            data = bytes(frame[7:15])
+            
+            if data_len < 8:
+                data = data[:data_len]
+                
+            return (can_id, data, cmd, ide, rtr)
+            
+        return None

05_software/real/sim2real/vendored/odin1_imu/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.16)
+project(odin1 LANGUAGES C CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+find_package(PkgConfig REQUIRED)
+find_package(OpenSSL REQUIRED)
+pkg_check_modules(LIBUSB REQUIRED libusb-1.0)
+
+add_library(odin1_imu_bridge SHARED
+    src/odin1_imu_bridge.cpp
+)
+
+target_include_directories(odin1_imu_bridge
+    PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${LIBUSB_INCLUDE_DIRS}
+)
+
+target_link_directories(odin1_imu_bridge
+    PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/lib
+)
+
+target_link_libraries(odin1_imu_bridge
+    PRIVATE
+        ${CMAKE_CURRENT_SOURCE_DIR}/lib/liblydHostApi_arm.a
+        ${LIBUSB_LIBRARIES}
+        OpenSSL::SSL
+        OpenSSL::Crypto
+        pthread
+        rt
+        dl
+)

05_software/real/sim2real/vendored/odin1_imu/build.sh

@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+BUILD_DIR="${SCRIPT_DIR}/build"
+
+cmake -S "${SCRIPT_DIR}" -B "${BUILD_DIR}" -DCMAKE_BUILD_TYPE=Release
+cmake --build "${BUILD_DIR}" -j"$(nproc)"

05_software/real/sim2real/vendored/odin1_imu/include/lidar_api.h

@@ -0,0 +1,308 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+#ifndef LIDAR_API_H
+#define LIDAR_API_H
+
+/**
+ * @file lidar_api.h
+ * @brief LiDAR device API for controlling and accessing LiDAR sensor data
+ * 
+ * This header provides the public interface for interacting with LiDAR devices.
+ * It includes functions for device management, data streaming control, and
+ * device configuration.
+ *
+ * @copyright Copyright (c) 2025, Manifold Tech Limited, All Rights Reserved
+ * @version 1.0
+ */
+
+#include "lidar_api_type.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief Initialize the LiDAR system
+ * 
+ * Must be called before any other lidar function to set up the system resources.
+ * 
+ * @param cb Callback function for device events (connection, disconnection)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_system_init(lidar_device_callback_t cb);
+
+/**
+ * @brief Deinitialize the LiDAR system
+ * 
+ * Releases all resources allocated by the system. Should be called when
+ * application is shutting down.
+ * 
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_system_deinit(void);
+
+/**
+ * @brief Create a handle for a LiDAR device
+ * 
+ * @param dev_info Information about the LiDAR device to create
+ * @param device Pointer to receive the device handle upon success
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_create_device(lidar_device_info_t *dev_info, device_handle *device);
+
+/**
+ * @brief Destroy a LiDAR device handle
+ * 
+ * Releases resources associated with the device handle. Must be called 
+ * when the device is no longer needed.
+ * 
+ * @param device Handle to the device to destroy
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_destory_device(device_handle device);
+
+/**
+ * @brief Register callback function for receiving LiDAR data streams
+ * 
+ * Sets up a callback function that will be called when new data is available.
+ * 
+ * @param device Handle to the target device
+ * @param cb Callback information containing function pointers for different data types
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_register_stream_callback(device_handle device, lidar_data_callback_info_t cb);
+
+/**
+ * @brief Unregister stream callback for a device
+ * 
+ * Stops the device from calling back when new data is available.
+ * 
+ * @param device Handle to the target device
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_unregister_stream_callback(device_handle device);
+
+/**
+ * @brief Open a LiDAR device for communication
+ * 
+ * Establishes a connection to the physical device.
+ * 
+ * @param device Handle to the device to open
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_open_device(device_handle device);
+
+/**
+ * @brief Close a LiDAR device
+ * 
+ * Closes the connection to the physical device.
+ * 
+ * @param device Handle to the device to close
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_close_device(device_handle device);
+
+/**
+ * @brief Set the operating mode of the LiDAR device
+ * 
+ * @param device Handle to the target device
+ * @param mode Operating mode to set (see mode definitions in lidar_api_type.h)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_mode(device_handle device, int mode);
+
+/**
+ * @brief Start data streaming from the device
+ * 
+ * Begins the flow of data from the device for the specified type.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to start (see stream type definitions in lidar_api_type.h)
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_start_stream(device_handle device, int type, uint32_t &dtof_subframe_odr);
+
+/**
+ * @brief Stop data streaming from the device
+ * 
+ * Stops the flow of data from the device for the specified type.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to stop
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_stop_stream(device_handle device, int type);
+
+/**
+ * @brief Activate a specific stream type on the device
+ * 
+ * Enables a specific data stream type in the device configuration.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to activate
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_activate_stream_type(device_handle device, int type);
+
+/**
+ * @brief Deactivate a specific stream type on the device
+ * 
+ * Disables a specific data stream type in the device configuration.
+ * 
+ * @param device Handle to the target device
+ * @param type Type of data stream to deactivate
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_deactivate_stream_type(device_handle device, int type);
+
+/**
+ * @brief Get calibration file from the device
+ * 
+ * Retrieves the calibration file from the device.
+ * 
+ * @param device Handle to the target device
+ * @param path Path to save the calibration file
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_calib_file(device_handle device, const char* path);
+
+/**
+ * @brief Set log verbosity level
+ * 
+ * Controls the amount of log information generated by the LiDAR API.
+ * 
+ * @param level Log level to set (see level definitions in lidar_api_type.h)
+ */
+void lidar_log_set_level(lidar_log_level_e level);
+
+/**
+ * @brief Get the version information of the LiDAR device
+ * 
+ * Retrieves version information including firmware, system, and application versions.
+ * 
+ * @param device Handle to the target device
+ * @param version struct Pointer to receive the version information
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_version(device_handle device,lidar_fireware_version_t *version);
+
+/**
+ * @brief Set custom algorithm parameters for the device
+ * 
+ * Sends custom parameter settings to the device.
+ *
+ * @param device Handle to the target device
+ * @param param_name String name of the parameter to set
+ * @param value_data Pointer to the value data to set for the parameter
+ * @param value_length Length of the value data in bytes
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_custom_parameter(device_handle device, const char* param_name, const void* value_data, size_t value_length);
+
+/**
+ * @brief Get custom algorithm parameters for the device
+ * 
+ * Get custom parameter settings from the device.
+ *
+ * @param device Handle to the target device
+ * @param param_name String name of the parameter to get
+ * @param value Integer value to get for the parameter
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_get_custom_parameter(device_handle device, const char* param_name, int* value);
+
+/**
+ * @brief Set the map file used for relocalization
+ *
+ * Read & send specified map file to device for relocalization
+ *
+ * @param device Handle to the target device
+ * @param abs_path Absolute path to the map file
+ * @return int 0 on success, otherwise on failure
+ */
+ int lidar_set_relocalization_map(device_handle device, const char* abs_path);
+
+ /**
+ * @brief Get the mapping result file from device
+ *
+ * Read & send specified map file from device to host
+ *
+ * @param device Handle to the target device
+ * @param dest_dir Destination directory to save the map file
+ * @param file_name File name to save the map file
+ * @return int 0 on success, -1 on failure without error code, error code (> 0) otherwise
+ */
+ int lidar_get_mapping_result(device_handle device, const char* dest_dir, const char* file_name);
+
+/**
+ * @brief Set the image mask file for the device
+ *
+ * Read & send specified image mask file to device
+ *
+ * @param device Handle to the target device
+ * @param abs_path Absolute path to the image mask file (e.g., mask.png)
+ * @return int 0 on success, -1 on failure, -2 if file transfer in progress
+ */
+ int lidar_set_image_mask(device_handle device, const char* abs_path);
+
+
+  /**
+ * @brief enable device log
+ *
+ *
+ * @param device Handle to the target device
+ * @param dest_dir Destination directory to save the logs
+ * @return int 0 on success, -1 on failure
+ */
+ int lidar_enable_encrypted_device_log(device_handle device, const char* dest_dir);
+
+
+/**
+ * @brief Set the depth parameters for the device
+ * 
+ * This function must be called before starting data stream.
+ *
+ * @param device Handle to the target device
+ * @param params Pointer to the depth parameters to set
+ * @return int 0 on success, negative error code on failure
+ */
+int lidar_set_depth_parameter(device_handle device, const lidar_depth_para_t *params);
+
+/**
+ * @brief Enable or disable IMU smooth sending feature
+ *
+ * When enabled, IMU data will be sent at precise intervals (default 400Hz) 
+ * using a dedicated high-priority thread to reduce jitter and timing variance.
+ * When disabled, IMU data will be sent immediately upon reception.
+ *
+ * @param enable 1 to enable smooth sending, 0 to disable
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_enable_imu_smooth_sending(int enable);
+
+/**
+ * @brief Set IMU smooth sending frequency
+ *
+ * Set the target frequency for IMU smooth sending. Only effective when 
+ * smooth sending is enabled via lidar_enable_imu_smooth_sending().
+ *
+ * @param frequency_hz Target frequency in Hz (1-1000 Hz, recommended 400 Hz)
+ * @return int 0 on success, -1 on failure
+ */
+int lidar_set_imu_smooth_frequency(uint32_t frequency_hz);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LIDAR_API_H

05_software/real/sim2real/vendored/odin1_imu/include/lidar_api_type.h

@@ -0,0 +1,242 @@
+/*
+Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+   http://www.apache.org/licenses/LICENSE-2.0
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+#ifndef LIDAR_TYPES_H
+#define LIDAR_TYPES_H
+
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define LIDAR_SERIAL_MAX 64
+#define LIDAR_MODEL_MAX  64
+#define LIDAR_IP_MAX     64
+
+typedef void * device_handle;
+
+typedef enum {
+    LIDAR_LOG_ERROR = 0,
+    LIDAR_LOG_WARN,
+    LIDAR_LOG_INFO,
+    LIDAR_LOG_DEBUG,
+} lidar_log_level_e;
+
+typedef enum {
+    LIDAR_OTA_ALGORITHM,
+    LIDAR_OTA_FIRMWARE,
+    LIDAR_OTA_SCRIPT,
+    LIDAR_OTA_CALIBRATION
+} lidar_ota_type_e;
+
+typedef enum {
+    LIDAR_MODE_RAW,
+    LIDAR_MODE_SLAM,
+} lidar_mode_e;
+
+typedef enum {
+    LIDAR_DT_NONE = 0,
+    LIDAR_DT_RAW_RGB,
+    LIDAR_DT_RAW_IMU,
+    LIDAR_DT_RAW_DTOF,
+    LIDAR_DT_SLAM_CLOUD,
+    LIDAR_DT_SLAM_ODOMETRY,
+    LIDAR_DT_DEV_STATUS,
+    LIDAR_DT_SLAM_ODOMETRY_HIGHFREQ,
+    LIDAR_DT_SLAM_ODOMETRY_TF,
+    LIDAR_DT_SLAM_WIWC,
+    LIDAR_DT_NTP
+} lidar_data_type_e;
+
+typedef struct {
+    int8_t serial[LIDAR_SERIAL_MAX];
+    int8_t model[LIDAR_MODEL_MAX];
+    bool online;
+    uint32_t initial_state;
+} lidar_device_info_t;
+
+typedef struct {
+    float x, y, z;
+    float intensity;
+} lidar_point_t;
+
+
+typedef struct {
+    float intrinsics[9];
+    float extrinsics[16];
+} lidar_calibration_t;
+
+
+#define DEVICE_MAX_CH_NUMBER 4
+
+typedef struct {
+    uint64_t timestamp_ns;
+    int64_t pos[3];
+    int64_t orient[4];
+} ros2_odom_convert_t;
+
+typedef struct {
+    uint64_t timestamp_ns;
+    int64_t pos[3];
+    int64_t orient[4];
+    int64_t linear_velocity[3];
+    int64_t angular_velocity[3];
+    double pose_cov[36];
+    double twist_cov[36];
+} ros_odom_convert_complete_t;
+
+typedef struct {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp;
+    uint64_t sequence;
+} imu_convert_data_t;
+
+ typedef struct {
+    uint32_t length;
+    uint64_t sequence;
+    uint64_t timestamp;
+    uint64_t interval;
+    void* pAddr;
+    uint32_t width;
+    uint32_t height;
+} buffer_List_t;
+
+typedef struct {
+    double delay;
+    double offset;
+} ptp_sync_data_t;
+
+typedef struct capture_Image_List_t {
+    uint32_t imageCount;
+    buffer_List_t imageList[DEVICE_MAX_CH_NUMBER];
+} capture_Image_List_t;
+
+typedef struct {
+    uint32_t type;
+    capture_Image_List_t stream;
+} lidar_data_t;
+
+typedef void (*lidar_device_callback_t)(const lidar_device_info_t* device, bool attach);
+typedef void (*lidar_data_callback_t)(const lidar_data_t *data, void *user_data);
+
+typedef struct {
+    lidar_data_callback_t data_callback;
+    void *user_data;
+} lidar_data_callback_info_t;
+
+typedef struct {
+    int major;
+    int minor;
+    int patch;
+}lidar_version_t;
+
+typedef struct {
+    lidar_version_t kernel_version;
+    lidar_version_t mcu_version;
+    lidar_version_t soc_version;
+    lidar_version_t Daemon_proc_version;
+    lidar_version_t slam_version;
+} lidar_fireware_version_t;
+
+/**
+ * @brief RGB image sensor frame rate
+ * 
+ */
+ typedef struct{
+
+    int configured_odr; /* rgb image sensor configured output data rate */
+    int tx_odr;         /* rgb image sensor tx output data rate */
+
+} lidar_rgb_sensor_status_t;
+
+/**
+ * @brief DTOF Lidar frame rate
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* dtof lidar sensor configured output data rate */
+    int tx_odr;         /* dtof lidar sensor tx output data rate */
+    int subframe_odr;   /* dtof lidar sensor subframe output data rate */
+    short tx_temp;      /* dtof lidar tx module temp */
+    short rx_temp;      /* dtof lidar rx module temp */
+
+} lidar_dtof_sensor_status_t;
+
+/**
+ * @brief IMU Sensor
+ * 
+ */
+typedef struct{
+
+    int configured_odr; /* imu sensor configured output data rate */
+    int tx_odr;         /* imu sensor tx output data rate */
+
+} lidar_imu_sensor_status_t;
+
+typedef struct{
+
+    int package_temp;   /* soc package temp */
+    int cpu_temp;       /* cpu temp */
+    int center_temp;    /* center temp */
+    int gpu_temp;       /* gpu temp */
+    int npu_temp;       /* npu temp */
+
+} lidar_soc_thermal_t;
+typedef struct
+{
+    double uptime_seconds;
+    lidar_soc_thermal_t soc_thermal; 
+
+    int cpu_use_rate[8];              /* cpu usage rate */
+    int ram_use_rate;                 /* ram usage rate */
+
+    lidar_rgb_sensor_status_t rgb_sensor;
+    lidar_dtof_sensor_status_t dtof_sensor;
+    lidar_imu_sensor_status_t imu_sensor;
+
+    int slam_cloud_tx_odr;          /* slam cloud tx output data rate */
+    int slam_odom_tx_odr;           /* slam odom tx output data rate */
+    int slam_odom_highfreq_tx_odr;  /* slam odom high freq tx output data rate */
+
+} lidar_device_status_t;
+
+typedef enum {
+    LIDAR_DEVICE_NONE = 0,
+    LIDAR_DEVICE_NOT_INITIALIZED,
+    LIDAR_DEVICE_INITIALIZED,
+    LIDAR_DEVICE_STREAMING,
+    LIDAR_DEVICE_STREAM_STOPPED,
+} lidar_device_initial_state_e;
+
+typedef enum {
+    LIDAR_DEPTH_ODR_10HZ = 0,
+    LIDAR_DEPTH_ODR_14_5HZ,
+} lidar_depth_odr_e;
+
+typedef struct {
+    lidar_depth_odr_e odr;
+} lidar_depth_para_t;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

05_software/real/sim2real/vendored/odin1_imu/include/odin1_imu_bridge.h

@@ -0,0 +1,86 @@
+#ifndef ODIN1_IMU_BRIDGE_H
+#define ODIN1_IMU_BRIDGE_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * 输入: 无
+ * 输出: odin1_imu_sample_t
+ * 作用: 描述一帧 IMU 数据, 供 C/C++/Python 共享使用
+ */
+typedef struct odin1_imu_sample_t {
+    float accel_x;
+    float accel_y;
+    float accel_z;
+    float gyro_x;
+    float gyro_y;
+    float gyro_z;
+    uint64_t stamp_ns;
+    uint64_t sequence;
+} odin1_imu_sample_t;
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回当前 bridge 的版本字符串
+ */
+const char* odin1_imu_version(void);
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 0 表示成功, 非 0 表示失败
+ * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
+ */
+int odin1_imu_start(int timeout_ms);
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 停止数据流并释放 SDK 资源
+ */
+void odin1_imu_stop(void);
+
+/**
+ * 输入: 无
+ * 输出: int, 1 表示运行中, 0 表示未运行
+ * 作用: 返回当前 bridge 是否处于运行状态
+ */
+int odin1_imu_is_running(void);
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待 IMU 数据到达
+ */
+int odin1_imu_wait_for_data(int timeout_ms);
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧 IMU 数据
+ */
+int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample);
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
+ */
+int odin1_imu_get_latest(odin1_imu_sample_t* out_sample);
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次错误信息
+ */
+const char* odin1_imu_last_error(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

05_software/real/sim2real/vendored/odin1_imu/odin1_imu.py

@@ -0,0 +1,124 @@
+#!/usr/bin/python3
+"""ODIN1 IMU ctypes 封装."""
+
+from __future__ import annotations
+
+import ctypes
+from pathlib import Path
+from typing import Iterator, Optional
+
+
+class Odin1ImuSample(ctypes.Structure):
+    """输入: 无; 输出: Odin1ImuSample; 作用: 映射 C++ bridge 的 IMU 结构体."""
+
+    _fields_ = [
+        ("accel_x", ctypes.c_float),
+        ("accel_y", ctypes.c_float),
+        ("accel_z", ctypes.c_float),
+        ("gyro_x", ctypes.c_float),
+        ("gyro_y", ctypes.c_float),
+        ("gyro_z", ctypes.c_float),
+        ("stamp_ns", ctypes.c_uint64),
+        ("sequence", ctypes.c_uint64),
+    ]
+
+
+class Odin1ImuClient:
+    """输入: lib_path[Optional[str|Path]]; 输出: Odin1ImuClient; 作用: 提供 Python 对 ODIN1 IMU bridge 的访问接口."""
+
+    def __init__(self, lib_path: Optional[str | Path] = None) -> None:
+        self._project_root = Path(__file__).resolve().parents[1]
+        resolved_path = Path(lib_path) if lib_path else self._project_root / "build" / "libodin1_imu_bridge.so"
+        self._lib = ctypes.CDLL(str(resolved_path))
+        self._configure_signatures()
+
+    def _configure_signatures(self) -> None:
+        """输入: 无; 输出: 无; 作用: 配置 ctypes 函数签名."""
+
+        self._lib.odin1_imu_version.restype = ctypes.c_char_p
+
+        self._lib.odin1_imu_start.argtypes = [ctypes.c_int]
+        self._lib.odin1_imu_start.restype = ctypes.c_int
+
+        self._lib.odin1_imu_stop.argtypes = []
+        self._lib.odin1_imu_stop.restype = None
+
+        self._lib.odin1_imu_is_running.argtypes = []
+        self._lib.odin1_imu_is_running.restype = ctypes.c_int
+
+        self._lib.odin1_imu_wait_for_data.argtypes = [ctypes.c_int]
+        self._lib.odin1_imu_wait_for_data.restype = ctypes.c_int
+
+        self._lib.odin1_imu_pop_sample.argtypes = [ctypes.POINTER(Odin1ImuSample)]
+        self._lib.odin1_imu_pop_sample.restype = ctypes.c_int
+
+        self._lib.odin1_imu_get_latest.argtypes = [ctypes.POINTER(Odin1ImuSample)]
+        self._lib.odin1_imu_get_latest.restype = ctypes.c_int
+
+        self._lib.odin1_imu_last_error.argtypes = []
+        self._lib.odin1_imu_last_error.restype = ctypes.c_char_p
+
+    def version(self) -> str:
+        """输入: 无; 输出: str; 作用: 获取 C++ bridge 版本号."""
+
+        return self._lib.odin1_imu_version().decode("utf-8")
+
+    def last_error(self) -> str:
+        """输入: 无; 输出: str; 作用: 获取最近一次 bridge 错误信息."""
+
+        return self._lib.odin1_imu_last_error().decode("utf-8")
+
+    def start(self, timeout_ms: int = 5000) -> None:
+        """输入: timeout_ms[int]; 输出: 无; 作用: 启动 IMU 数据接收."""
+
+        result = self._lib.odin1_imu_start(timeout_ms)
+        if result != 0:
+            raise RuntimeError(f"启动 ODIN1 IMU 失败: {self.last_error()} (code={result})")
+
+    def stop(self) -> None:
+        """输入: 无; 输出: 无; 作用: 停止 IMU 数据接收."""
+
+        self._lib.odin1_imu_stop()
+
+    def is_running(self) -> bool:
+        """输入: 无; 输出: bool; 作用: 返回 bridge 是否仍在运行."""
+
+        return bool(self._lib.odin1_imu_is_running())
+
+    def wait_for_data(self, timeout_ms: int = 1000) -> bool:
+        """输入: timeout_ms[int]; 输出: bool; 作用: 等待 IMU 数据到达."""
+
+        result = self._lib.odin1_imu_wait_for_data(timeout_ms)
+        if result < 0:
+            raise RuntimeError(f"等待 IMU 数据失败: {self.last_error()} (code={result})")
+        return bool(result)
+
+    def pop_sample(self) -> Optional[Odin1ImuSample]:
+        """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 从队列中取出一帧 IMU 数据."""
+
+        sample = Odin1ImuSample()
+        result = self._lib.odin1_imu_pop_sample(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取 IMU 队列失败: {self.last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    def get_latest(self) -> Optional[Odin1ImuSample]:
+        """输入: 无; 输出: Optional[Odin1ImuSample]; 作用: 获取最近一帧 IMU 数据."""
+
+        sample = Odin1ImuSample()
+        result = self._lib.odin1_imu_get_latest(ctypes.byref(sample))
+        if result < 0:
+            raise RuntimeError(f"读取最新 IMU 数据失败: {self.last_error()} (code={result})")
+        return sample if result == 1 else None
+
+    def iter_samples(self, timeout_ms: int = 1000) -> Iterator[Odin1ImuSample]:
+        """输入: timeout_ms[int]; 输出: Iterator[Odin1ImuSample]; 作用: 连续迭代输出 IMU 数据."""
+
+        while self.is_running():
+            if not self.wait_for_data(timeout_ms):
+                continue
+            while True:
+                sample = self.pop_sample()
+                if sample is None:
+                    break
+                yield sample

05_software/real/sim2real/vendored/odin1_imu/src/odin1_imu_bridge.cpp

@@ -0,0 +1,376 @@
+#include "odin1_imu_bridge.h"
+
+#include "lidar_api.h"
+#include "lidar_api_type.h"
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <cstring>
+#include <deque>
+#include <mutex>
+#include <string>
+#include <thread>
+
+namespace {
+
+constexpr const char* kBridgeVersion = "0.1.0";
+constexpr std::size_t kMaxQueueSize = 1024;
+constexpr int kDefaultMode = LIDAR_MODE_SLAM;
+
+std::atomic<bool> g_running{false};
+std::atomic<bool> g_sdk_initialized{false};
+std::atomic<bool> g_device_connected{false};
+std::atomic<bool> g_stream_started{false};
+
+device_handle g_device = nullptr;
+
+std::mutex g_state_mutex;
+std::mutex g_queue_mutex;
+std::condition_variable g_queue_cv;
+std::deque<odin1_imu_sample_t> g_queue;
+odin1_imu_sample_t g_latest_sample{};
+bool g_has_latest_sample = false;
+
+std::mutex g_error_mutex;
+std::string g_last_error = "bridge not started";
+
+/**
+ * 输入: message[const std::string&]
+ * 输出: 无
+ * 作用: 线程安全地记录最近一次错误信息
+ */
+void set_last_error(const std::string& message) {
+    std::lock_guard<std::mutex> lock(g_error_mutex);
+    g_last_error = message;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 清空内部 IMU 队列和最近一帧缓存
+ */
+void clear_queue_locked_state() {
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    g_queue.clear();
+    g_latest_sample = {};
+    g_has_latest_sample = false;
+}
+
+/**
+ * 输入: raw_sample[const imu_convert_data_t*]
+ * 输出: odin1_imu_sample_t
+ * 作用: 将 SDK IMU 结构转换为 bridge 对外结构
+ */
+odin1_imu_sample_t convert_sample(const imu_convert_data_t* raw_sample) {
+    odin1_imu_sample_t converted{};
+    if (raw_sample == nullptr) {
+        return converted;
+    }
+
+    converted.accel_x = raw_sample->accel_x;
+    converted.accel_y = raw_sample->accel_y;
+    converted.accel_z = raw_sample->accel_z;
+    converted.gyro_x = raw_sample->gyro_x;
+    converted.gyro_y = raw_sample->gyro_y;
+    converted.gyro_z = raw_sample->gyro_z;
+    converted.stamp_ns = raw_sample->stamp;
+    converted.sequence = raw_sample->sequence;
+    return converted;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 安全关闭当前设备与 SDK 资源
+ */
+void cleanup_device_and_sdk() {
+    std::lock_guard<std::mutex> lock(g_state_mutex);
+
+    if (g_device != nullptr) {
+        try {
+            if (g_stream_started.load()) {
+                lidar_deactivate_stream_type(g_device, LIDAR_DT_RAW_IMU);
+                lidar_stop_stream(g_device, kDefaultMode);
+                g_stream_started = false;
+            }
+
+            lidar_unregister_stream_callback(g_device);
+            lidar_close_device(g_device);
+            lidar_destory_device(g_device);
+        } catch (...) {
+        }
+        g_device = nullptr;
+    }
+
+    if (g_sdk_initialized.load()) {
+        try {
+            lidar_system_deinit();
+        } catch (...) {
+        }
+        g_sdk_initialized = false;
+    }
+
+    g_device_connected = false;
+}
+
+/**
+ * 输入: data[const lidar_data_t*], user_data[void*]
+ * 输出: 无
+ * 作用: 接收 SDK 回调中的 IMU 数据并写入内部缓存队列
+ */
+void lidar_data_callback(const lidar_data_t* data, void* user_data) {
+    (void)user_data;
+
+    if (!g_running.load() || data == nullptr) {
+        return;
+    }
+
+    if (data->type != LIDAR_DT_RAW_IMU) {
+        return;
+    }
+
+    if (data->stream.imageList[0].pAddr == nullptr) {
+        set_last_error("sdk imu callback returned null payload");
+        return;
+    }
+
+    const auto* raw_sample =
+        static_cast<const imu_convert_data_t*>(data->stream.imageList[0].pAddr);
+    odin1_imu_sample_t sample = convert_sample(raw_sample);
+
+    {
+        std::lock_guard<std::mutex> lock(g_queue_mutex);
+        if (g_queue.size() >= kMaxQueueSize) {
+            g_queue.pop_front();
+        }
+        g_queue.push_back(sample);
+        g_latest_sample = sample;
+        g_has_latest_sample = true;
+    }
+
+    g_queue_cv.notify_all();
+}
+
+/**
+ * 输入: device_info[const lidar_device_info_t*], attach[bool]
+ * 输出: 无
+ * 作用: 响应 SDK 设备插拔事件并启动 IMU 数据流
+ */
+void lidar_device_callback(const lidar_device_info_t* device_info, bool attach) {
+    if (!g_running.load()) {
+        return;
+    }
+
+    if (!attach) {
+        g_device_connected = false;
+        g_stream_started = false;
+        return;
+    }
+
+    if (device_info == nullptr) {
+        set_last_error("sdk device callback returned null device info");
+        return;
+    }
+
+    std::lock_guard<std::mutex> lock(g_state_mutex);
+
+    if (g_device != nullptr) {
+        return;
+    }
+
+    device_handle device_handle_local = nullptr;
+    if (lidar_create_device(const_cast<lidar_device_info_t*>(device_info), &device_handle_local) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_create_device failed");
+        return;
+    }
+
+    if (lidar_open_device(device_handle_local) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_open_device failed");
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    lidar_data_callback_info_t callback_info{};
+    callback_info.data_callback = lidar_data_callback;
+    callback_info.user_data = nullptr;
+    if (lidar_register_stream_callback(device_handle_local, callback_info) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_register_stream_callback failed");
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    uint32_t dtof_subframe_odr = 0;
+    if (lidar_start_stream(device_handle_local, kDefaultMode, dtof_subframe_odr) != 0) {  // SDK接口，来源: include/lidar_api.h
+        (void)dtof_subframe_odr;
+        set_last_error("lidar_start_stream failed");
+        lidar_unregister_stream_callback(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    if (lidar_activate_stream_type(device_handle_local, LIDAR_DT_RAW_IMU) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_activate_stream_type(raw_imu) failed");
+        lidar_stop_stream(device_handle_local, kDefaultMode);  // SDK接口，来源: include/lidar_api.h
+        lidar_unregister_stream_callback(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_close_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        lidar_destory_device(device_handle_local);  // SDK接口，来源: include/lidar_api.h
+        return;
+    }
+
+    g_device = device_handle_local;
+    g_stream_started = true;
+    g_device_connected = true;
+    set_last_error("");
+    g_queue_cv.notify_all();
+}
+
+}  // namespace
+
+extern "C" {
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回当前 bridge 的版本字符串
+ */
+const char* odin1_imu_version(void) {
+    return kBridgeVersion;
+}
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 0 表示成功, 非 0 表示失败
+ * 作用: 初始化 SDK, 等待设备连接并开始 IMU 数据流
+ */
+int odin1_imu_start(int timeout_ms) {
+    if (timeout_ms <= 0) {
+        timeout_ms = 5000;
+    }
+
+    if (g_running.load()) {
+        return 0;
+    }
+
+    clear_queue_locked_state();
+    set_last_error("waiting for odin1 device");
+
+    if (lidar_system_init(lidar_device_callback) != 0) {  // SDK接口，来源: include/lidar_api.h
+        set_last_error("lidar_system_init failed");
+        return -1;
+    }
+
+    g_sdk_initialized = true;
+    g_running = true;
+
+    const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms);
+    while (std::chrono::steady_clock::now() < deadline) {
+        if (g_device_connected.load()) {
+            return 0;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    }
+
+    set_last_error("timeout waiting for odin1 imu stream");
+    odin1_imu_stop();
+    return -2;
+}
+
+/**
+ * 输入: 无
+ * 输出: 无
+ * 作用: 停止数据流并释放 SDK 资源
+ */
+void odin1_imu_stop(void) {
+    g_running = false;
+    cleanup_device_and_sdk();
+    clear_queue_locked_state();
+    g_queue_cv.notify_all();
+}
+
+/**
+ * 输入: 无
+ * 输出: int, 1 表示运行中, 0 表示未运行
+ * 作用: 返回当前 bridge 是否处于运行状态
+ */
+int odin1_imu_is_running(void) {
+    return g_running.load() ? 1 : 0;
+}
+
+/**
+ * 输入: timeout_ms[int]
+ * 输出: int, 1 表示有数据可读, 0 表示超时, 负数表示异常
+ * 作用: 阻塞等待 IMU 数据到达
+ */
+int odin1_imu_wait_for_data(int timeout_ms) {
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    std::unique_lock<std::mutex> lock(g_queue_mutex);
+    const bool ready = g_queue_cv.wait_for(
+        lock,
+        std::chrono::milliseconds(timeout_ms > 0 ? timeout_ms : 1000),
+        [] { return !g_queue.empty() || !g_running.load(); });
+
+    if (!g_running.load()) {
+        return -1;
+    }
+
+    return ready && !g_queue.empty() ? 1 : 0;
+}
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功取出一帧, 0 表示队列为空, 负数表示异常
+ * 作用: 从内部队列中弹出一帧 IMU 数据
+ */
+int odin1_imu_pop_sample(odin1_imu_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_last_error("odin1_imu_pop_sample received null output pointer");
+        return -1;
+    }
+
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    if (g_queue.empty()) {
+        return 0;
+    }
+
+    *out_sample = g_queue.front();
+    g_queue.pop_front();
+    return 1;
+}
+
+/**
+ * 输入: out_sample[odin1_imu_sample_t*]
+ * 输出: int, 1 表示成功读取, 0 表示当前还没有数据, 负数表示异常
+ * 作用: 获取最近一帧 IMU 数据, 不会从队列中删除
+ */
+int odin1_imu_get_latest(odin1_imu_sample_t* out_sample) {
+    if (out_sample == nullptr) {
+        set_last_error("odin1_imu_get_latest received null output pointer");
+        return -1;
+    }
+
+    std::lock_guard<std::mutex> lock(g_queue_mutex);
+    if (!g_has_latest_sample) {
+        return 0;
+    }
+
+    *out_sample = g_latest_sample;
+    return 1;
+}
+
+/**
+ * 输入: 无
+ * 输出: const char*
+ * 作用: 返回最近一次错误信息
+ */
+const char* odin1_imu_last_error(void) {
+    std::lock_guard<std::mutex> lock(g_error_mutex);
+    return g_last_error.c_str();
+}
+
+}  // extern "C"