Low Level (RL 정책) — C++ 예제

소스: rbq_sdk/cpp/example/src/rbq_low_level.cpp

IMU, 조이스틱, 다리 관절 피드백 토픽을 구독하고, 관절 위치 레퍼런스와 소유권 명령을 발행하며, 선택적으로 학습된 보행 정책(ONNX)을 실행하는 저수준 관절 제어 예제입니다.

동작 개요

애플리케이션은 단일 500 Hz 제어 루프 (2 ms 주기)와 키보드 기반 상태 머신으로 구성됩니다:

상태	키	제어 루프 동작
Idle	—	스택의 최종 레퍼런스(rt/rbq/ref/motion/_0)를 추적하고 재발행 — 투명한 패스스루 역할
Motion	x / z	JointControl 코사인 궤적을 사용해 ready 또는 ground 자세로 관절 보간
Control	c	ONNX 정책 로드, 100 Hz로 추론 실행 (2 ms 루프의 5번 데시메이션), 정책 게인으로 MotionRef 발행

사전 요구사항

• RBQ SDK 빌드 및 설치 완료 — C++ SDK 개요 참조.
• onnxruntime 라이브러리 — rbq_sdk/cpp/example/scripts/setup.bash 로 설치됩니다.
• info.json과 policy.onnx가 들어있는 정책 디렉토리.
• 로봇 스택이 실행 중(시뮬레이션 또는 하드웨어).

빌드

rbq_sdk/cpp/example/에서:

bash scripts/setup.bash
bash scripts/build.bash

바이너리: bin/rbq_low_level

실행

rbq_sdk/cpp/example/에서:

# RT 스레드를 위해 root 권한 필요; sudo 없이 실행하면 일반 스레드 사용
sudo ./bin/rbq_low_level -p <정책디렉토리> [networkInterface]

# 시뮬레이터 / 로봇 내부 스택 — 인터페이스 기본값은 lo
sudo ./bin/rbq_low_level -p ../../../rbq_simulator/rbq_gym/policy/rbq10/

# 실제 로봇 — 로봇에 닿는 인터페이스를 넘깁니다
sudo ./bin/rbq_low_level -p ../../../rbq_simulator/rbq_gym/policy/rbq10/ eth0

-p 옵션은 필수입니다. 없으면 프로그램이 에러와 함께 종료됩니다.
networkInterface 는 선택이며 기본값은 lo 로, 시뮬레이터와 로봇 내부 스택이 쓰는 인터페이스와 같습니다.

시뮬레이션으로 실행

1. RBQ/에서 전체 스택 시작:

   bash scripts/sim.bash --no-gui

2. 같은 PC 에서 RBQ 앱을 켭니다. 데스크탑 앱이 이 시뮬레이터를 스스로 찾아 붙습니다 — 컨트롤 → 자동 기동 → STAND 순서로 누릅니다. 제어 홈 참고.
3. rbq_sdk/cpp/example/에서:

   sudo ./bin/rbq_low_level -p ../../../rbq_simulator/rbq_gym/policy/rbq10/

키보드 제어

키	동작
q	종료
z	ground 자세로 이동 (접기 → 지면, ~4.8 s)
x	ready 자세로 이동 (지면 → 접기 → 준비, ~2.8–5.6 s)
c	Walk / Control 모드 시작 (-p 경로의 정책 로드)

비캐노니컬 터미널 I/O(non-canonical, no echo)를 사용하므로 Enter 없이 키 입력 즉시 반응합니다.

정책 디렉토리

-p 디렉토리에는 다음 파일들이 있어야 합니다:

파일	용도
info.json	정책 하이퍼파라미터 (run_name, policy_dt, action_scale, 강성/댐핑, 관측/행동 크기, 정규화 스케일, 기본 관절 각도, 명령 범위)
policy.onnx	학습 프레임워크에서 내보낸 ONNX 모델

지원되는 정책 이름 (info.json의 run_name):

이름	관측 벡터	비고
rbq10	gyro, proj_grav, command, dof_pos, dof_vel, action	단일 프레임
rbq10_trot	gyro, proj_grav, command, 4×dof_pos 히스토리(0,2,4,6), 4×dof_vel 히스토리, action×2, added_mass	7프레임 히스토리, 데시메이션 5
rbq10_trot_run	rbq10_trot과 동일	고속 런 변형

DDS 토픽

방향	토픽	타입	설명
구독	rt/rbq/joy	Joy_	속도 명령용 조이스틱 축 값
구독	rt/rbq/imu	Imu_	각속도 + 자세 쿼터니언
구독	rt/rbq/leg_joint	LegJointInfo_	관절 위치 및 속도 (12개)
구독	rt/rbq/ref/motion/_0	MotionRef_	최종 블렌딩 레퍼런스 (Idle에서 사용)
발행	rt/rbq/ref/motion/_20	MotionRef_	PD 게인이 포함된 관절 위치 레퍼런스
발행	rt/rbq/cmd/motion/owner/_20	JointOwnershipCmd_	12개 관절 소유권 요청

관절 인덱스 순서

관절은 0–11 순서로 인덱싱됩니다:

HR (후우측): HRR(0), HRP(1), HRK(2) →
HL (후좌측): HLR(3), HLP(4), HLK(5) →
FR (전우측): FRR(6), FRP(7), FRK(8) →
FL (전좌측): FLR(9), FLP(10), FLK(11)

주요 구현 사항

• RT 스레드: root로 실행 시 rbq_sdk::Thread::generate_rt_thread를 사용해 SCHED_FIFO 스케줄링을 적용합니다. root 없이 실행하면 일반 std::thread를 사용합니다.
• 소유권(Ownership): Motion 또는 Control 상태로 전환할 때 JointOwnershipCmd_를 한 번 발행하여 12개 관절 전체를 요청합니다 (owner_20 우선순위).
• 자세 감지: goToMotionReady()는 4개 고관절 피치 각도를 60° 기준으로 비교해 로봇이 현재 지면에 접혀있는지 확인하고, 필요시 접기 단계를 추가합니다.
• 데시메이션: 정책은 2 ms 루프에서 5번에 한 번 (100 Hz)씩 실행되며, 이는 info.json의 policy_dt와 일치합니다.

문제 해결

문제	가능한 원인
움직임 없음 / 토픽 없음	스택이 실행 중이지 않거나 잘못된 네트워크 인터페이스
Policy, PolicyParams not loaded	-p 경로 오류, info.json/policy.onnx 누락 또는 잘못된 형식
Thread deadline … over!	2 ms 주기 유지 불가 — sudo로 실행(RT 스레드) 또는 시스템 부하 감소
apt 인증 오류	호스트 GPG 문제와 무관; 별도로 수정 필요

관련 문서

• High Level Command
• C++ SDK 개요
• 개념 설명 — 저수준 / LV0