Low Level (RL 정책) — C++ 예제
소스: rbq_sdk/cpp/example/src/rbq_low_level.cpp
IMU, 조이스틱, 다리 관절 피드백 토픽을 구독하고, 관절 위치 레퍼런스와 소유권 명령을 발행하며, 선택적으로 **학습된 보행 정책(ONNX)**을 실행하는 저수준 관절 제어 예제입니다.
동작 개요
애플리케이션은 단일 500 Hz 제어 루프 (2 ms 주기)와 키보드 기반 상태 머신으로 구성됩니다:
| 상태 | 키 | 제어 루프 동작 |
|---|---|---|
| Idle | — | 스택의 최종 레퍼런스(rt/rbq/ref/leg_joint/final)를 추적하고 재발행 — 투명한 패스스루 역할 |
| Motion | x / z | JointControl 코사인 궤적을 사용해 ready 또는 ground 자세로 관절 보간 |
| Control | c | ONNX 정책 로드, 10 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
bash scripts/setup.bash
bash scripts/build.bash바이너리: bin/rbq_low_level
실행
rbq_sdk/cpp/example/에서:
bash
# RT 스레드를 위해 root 권한 필요; sudo 없이 실행하면 일반 스레드 사용
sudo ./bin/rbq_low_level -p <정책디렉토리>
# 사전 학습된 정책 사용:
sudo ./bin/rbq_low_level -p ../../rbq_simulator/rbq_gym/policy/rbq10/-p 옵션은 필수입니다. 없으면 프로그램이 에러와 함께 종료됩니다.
시뮬레이션으로 실행
RBQ/에서 전체 스택 시작:bashbash scripts/sim.bash- RBQGUI에서 Connect → Auto Start Robot → Stance (
2). rbq_sdk/cpp/example/에서:bashsudo ./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/cmd/joy/final | Joy_ | 속도 명령용 조이스틱 축 값 |
| 구독 | rt/rbq/info/imu | Imu_ | 각속도 + 자세 쿼터니언 |
| 구독 | rt/rbq/info/leg_joint | LegJointInfo_ | 관절 위치 및 속도 (12개) |
| 구독 | rt/rbq/ref/leg_joint/final | MotionRef_ | 최종 블렌딩 레퍼런스 (Idle에서 사용) |
| 발행 | rt/rbq/ref/leg_joint/owner_20 | MotionRef_ | PD 게인이 포함된 관절 위치 레퍼런스 |
| 발행 | rt/rbq/cmd/motion/joint_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번에 한 번 (10 Hz)씩 실행되며, 이는
info.json의policy_dt와 일치합니다.
문제 해결
| 문제 | 가능한 원인 |
|---|---|
| 움직임 없음 / 토픽 없음 | 스택이 실행 중이지 않거나 잘못된 네트워크 인터페이스 |
Policy, PolicyParams not loaded | -p 경로 오류, info.json/policy.onnx 누락 또는 잘못된 형식 |
Thread deadline … over! | 2 ms 주기 유지 불가 — sudo로 실행(RT 스레드) 또는 시스템 부하 감소 |
| apt 인증 오류 | 호스트 GPG 문제와 무관; 별도로 수정 필요 |