소프트웨어 스택

PC의 Isaac Lab 학습 환경에서 Rock 5C의 ONNX 추론 + Raspberry Pi Pico 2의 1kHz servo loop까지 end-to-end로 흘러갑니다.

1. 시스템 레이어 구조

2. ROS2 노드 토폴로지

                     ┌─────────────────────────────────────────┐
                     │              mechadog/                   │
                     │  ┌─────────┐    ┌──────────────┐         │
                     │  │ crsf_rx  │───►│  /cmd_vel    │ (Twist) │
                     │  │ (CRSF    │    └────┬─────────┘         │
                     │  │ parser)  │         │                   │
                     │  └─────────┘         ▼                   │
                     │              ┌──────────────┐            │
                     │              │ rl_policy_   │            │
                     │              │ onnx_runner  │            │
                     │              │ (50 Hz)      │            │
                     │              └────┬─────────┘            │
                     │                   │                      │
                     │   /joint_states ◄──┤                      │
                     │  /imu/data     ◄──┤──┐                  │
                     │  /camera/image ◄──┤  │                   │
                     │                   │  │                   │
                     │                   ▼  ▼                   │
                     │            ┌──────────────┐              │
                     │            │ pose_solver  │              │
                     │            │ /joint_cmds  │              │
                     │            └────┬─────────┘              │
                     │                 │   (SerialToPico)       │
                     └─────────────────┼─────────────────────────┘
                                       ▼
                              [Pico 2 over USB-CDC @ 115200bps]
                                       ▼
                                 [12× MG90S servo via PCA9685]

3. 의존성 — install on Rock 5C (aarch64)

# Debian 12 on Radxa Rock 5C Lite 8GB (Bookworm)
sudo apt update && sudo apt upgrade -y

# ROS2 Humble (Boot to LTS)
sudo apt install -y software-properties-common curl
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/keys/ros-keyring.gpg -o /usr/share/keyrings/ros-keyring.gpg
echo "deb [signed-by=/usr/share/keyrings/ros-keyring.gpg] http://packages.ros.org/ros2/ubuntu bookworm main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
sudo apt update
sudo apt install -y ros-humble-desktop ros-humble-ros-base python3-colcon-common-extensions

# Cyclone DDS middleware (작은 footprint + 333Hz RC input에 적합)
sudo apt install -y ros-humble-rmw-cyclonedds-cpp
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

# Python AI stack (Rock 5C에서 학습은 안 함; 배포만)
pip install --break-system-packages \
    onnxruntime==1.18.0 \
    rknn-toolkit2==1.6.0+8faa44c4-cp310-cp310-linux_aarch64_whl   # (RKNN NPU) — 실제 whl 이름은 git page에서 확인
pip install --break-system-packages pyserial numpy ultralytics==8.0.0

# Pi Camera v3 (CSI) - Armbian에서 자동 활성화
sudo apt install -y libcamera-apps ros-humble-camera-info-manager

4. 펌웨어·SDK 라인업 — MCU (Pico 2)

Pico 2는 마이크로파이썬으로 시작하는 것보다 C SDK + Pico SDK 2.x + tinyusb로 서보 루프 펌웨어를 작성하는 것을 권장합니다. 이유:

• 50 Hz 정책 → 12개 서보 PW를 정확히 동기
• 1 kHz PID loop(필요 시) — MicroPython은 너무 느림
• USB-CDC dual port로 Rock 5C와 ROS2 메시지 통신

// Pico 2 SDK 2.x initialization example (1kHz servo loop)
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "hardware/pwm.h"

#define I2C_PORT i2c0
#define SDA_PIN 0
#define SCL_PIN 1
#define PWM_FREQ_HZ 50

uint16_t angle_to_pwm(float deg) {
    // MG90S: 1000-2000us, 0-180 deg
    float us = 1000.0f + (deg * 1000.0f) / 180.0f;
    return (uint16_t)(us * PWM_FREQ_HZ * 4096 / 1000000);  // PCA9685 tick
}

void pca_set_servo(uint8_t ch, float deg) {
    uint8_t buf[4] = {0x06, 0x00, 0x00, 0x00};
    uint16_t tick = angle_to_pwm(deg);
    buf[1] = tick & 0xFF;
    buf[2] = (tick >> 8) & 0x0F;
    i2c_write_blocking(I2C_PORT, 0x40, buf, 4, false);
}

int main() {
    stdio_init_all();
    i2c_init(I2C_PORT, 400000);   // 400kHz I2C fast mode
    gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);

    // PCA9685 setup
    uint8_t mode1[2] = {0x00, 0x10};    // sleep
    i2c_write_blocking(I2C_PORT, 0x40, mode1, 2, false);
    uint8_t prescale[2] = {0xFE, (uint8_t)(25000000 / (4096 * PWM_FREQ_HZ) - 1)};
    i2c_write_blocking(I2C_PORT, 0x40, prescale, 2, false);
    uint8_t wake[2] = {0x00, 0xA1};     // restart + AI
    i2c_write_blocking(I2C_PORT, 0x40, wake, 2, false);
    sleep_ms(5);

    while (true) {
        for (uint8_t ch = 0; ch < 12; ch++) {
            pca_set_servo(ch, 90.0f);
        }
        sleep_ms(20);
    }
}

5. 데이터 흐름 — 카메라 인지 → 정책 입력

카메라 옵션 A(로그텍 C270 USB) 또는 옵션 B(Pi Camera v3 CSI)를 선택합니다.

• 옵션 A: C270 USB UVC → v4l2 → OpenCV → YOLO — USB 대역폭 480p 한정
• 옵션 B: Pi Camera v3 CSI → libcamera → RKNN YOLO-Nano (NPU) — 1080p 30fps 가능

카메라 결과는 ROS2 topic /detections로 통일하고, rl_policy_onnx_runner의 observation 일부( obs<0:4> = base state, obs<4:7> = camera + detected obstacle scrim )로 합칩니다. 학습 시뮬에서는 카메라를 플레이스홀더(제로 + instr)가 아니라, 시뮬레이션 내 synthetic depth map + heuristic obstacle box + ImageNet pretrained feature로 대체해 도메인 격차를 흡수합니다.

6. 외부 충격 저항 자세 보정 — 손쉬운 이론

외부 충격은 본체에 가해지는 dist torque τ_ext입니다. RL 정책은 평상시에는 τ_ext=0이라고 가정하지만, 학습 시에 다음을 함께 분포에 넣어 노출시켜야 합니다:

• 본체 임의 방향 0.5초 push 임펄스(0.3–1.5 N·s)
• 다리 접지 마찰 ±25% 변동
• 베이스 질량 ±10%
• IMU 잡음 +0.2 deg/s

이는 Humanoid-Gym 논문의 MoB(Mixture of Behaviors) + randomized impulse-on-reset 패턴으로 효용이 검증되어 있습니다.

7. 데이터 흐름 전체 pipeline

1. RC 입력: ELRS R24-P6 → UART4 → CRSF 파싱 → /cmd_vel (Twist)
2. 카메라 입력: Pi Camera → YOLO-Nano(RKNN NPU) → /detections (Detection[])
3. 관측치: 위 2 + IMU + joint_state → 48-dim vector → ONNX 정책 입력
4. 정책: 256-128-64 MLP → 12-dim action (관절 위치 오프셋 ±0.5 rad)
5. 저수준: ROS2 → CycloneDDS over USB-CDC → Pico 2 → PCA9685 → MG90S ×12
6. 외부 충격: EKF로 base_state 복원 → 정책의 자세 보정 즉시 적용

8. 다음 단계

• Isaac Lab에서 PPO 학습 →
• CRSF 파서 + ONNX 배포 →