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 <정책디렉토리>
# 사전 학습된 정책 사용:
sudo ./bin/rbq_low_level -p ../../rbq_simulator/rbq_gym/policy/rbq10/

-p 옵션은 필수입니다. 없으면 프로그램이 에러와 함께 종료됩니다.

시뮬레이션으로 실행

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

   bash scripts/sim.bash

2. RBQGUI에서 Connect → Auto Start Robot → Stance (2).
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