Low Level (RL Policy) — C++ Example

Source: rbq_sdk/cpp/example/src/rbq_low_level.cpp

Low-level joint control example that subscribes to IMU, joystick, and leg-joint feedback topics, publishes joint position references and ownership commands, and optionally runs a learned walking policy (ONNX).

Behavior overview

The application has a single 500 Hz control loop (2 ms period) and a keyboard-driven state machine with three states:

State	Key	What the control loop does
Idle	—	Tracks the stack's final reference (rt/rbq/ref/leg_joint/final) and republishes it — acts as a transparent pass-through
Motion	x / z	Interpolates joints to ready or ground postures using JointControl cosine trajectories
Control	c	Loads the ONNX policy, runs inference at 10 Hz (decimation 5 of the 2 ms loop), publishes MotionRef with policy gains

Prerequisites

• RBQ SDK built and installed — see C++ SDK Overview.
• onnxruntime library — installed by rbq_sdk/cpp/example/scripts/setup.bash.
• A policy directory containing info.json and policy.onnx.
• Robot stack running (simulation or hardware).

Build

From rbq_sdk/cpp/example/:

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

Binary: bin/rbq_low_level

Run

From rbq_sdk/cpp/example/:

# Requires root for RT thread; use without sudo for a normal thread
sudo ./bin/rbq_low_level -p <policyDir>
# Using pre-trained policy from rbq_simulator:
sudo ./bin/rbq_low_level -p ../../rbq_simulator/rbq_gym/policy/rbq10/

The application requires -p. Without it the program exits with an error.

Run with simulation

1. From RBQ/, start the full stack:

   bash scripts/sim.bash

2. In RBQGUI: Connect → Auto Start Robot → Stance (2).
3. From rbq_sdk/cpp/example/:

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

Keyboard controls

Key	Action
q	Quit
z	Go to ground posture (folding → ground, ~4.8 s)
x	Go to ready posture (ground → folding → ready, ~2.8–5.6 s)
c	Start Walk / Control mode (loads policy from -p)

Controls use raw terminal I/O (non-canonical, no echo) so each keypress is acted on immediately without pressing Enter.

Policy directory

The -p directory must contain:

File	Purpose
info.json	Policy hyper-parameters (run_name, policy_dt, action_scale, stiffness/damping, observation/action sizes, normalization scales, default joint angles, command ranges)
policy.onnx	ONNX model exported from the training framework

Supported policy names (from info.json run_name):

Name	Observation vector	Notes
rbq10	gyro, proj_grav, command, dof_pos, dof_vel, action	Single-frame
rbq10_trot	gyro, proj_grav, command, 4×dof_pos history (0,2,4,6), 4×dof_vel history, action×2, added_mass	7-frame history; decimation 5
rbq10_trot_run	Same as rbq10_trot	Faster run variant

DDS topics

Direction	Topic	Type	Description
Subscribe	rt/rbq/cmd/joy/final	Joy_	Joystick axes for velocity commands
Subscribe	rt/rbq/info/imu	Imu_	Angular velocity + orientation quaternion
Subscribe	rt/rbq/info/leg_joint	LegJointInfo_	Joint position and velocity (12 joints)
Subscribe	rt/rbq/ref/leg_joint/final	MotionRef_	Final blended reference (used in Idle)
Publish	rt/rbq/ref/leg_joint/owner_20	MotionRef_	Joint position references with PD gains
Publish	rt/rbq/cmd/motion/joint_owner/_20	JointOwnershipCmd_	Claim ownership of all 12 joints

Joint ordering

Joints are indexed 0–11 in the order:

HR (hind-right): HRR(0), HRP(1), HRK(2) →
HL (hind-left): HLR(3), HLP(4), HLK(5) →
FR (front-right): FRR(6), FRP(7), FRK(8) →
FL (front-left): FLR(9), FLP(10), FLK(11)

Key implementation notes

• RT thread: When running as root, rbq_sdk::Thread::generate_rt_thread is used for SCHED_FIFO scheduling. Without root the example falls back to a standard std::thread.
• Ownership: JointOwnershipCmd_ is published once when transitioning into Motion or Control state, claiming all 12 joints (owner_20). The stack respects ownership priority — the example has priority 20.
• Posture detection: goToMotionReady() checks the four hip-pitch angles against 60° to decide whether the robot is currently grounded (folded), and inserts a folding step if needed.
• Decimation: The policy runs every 5th control tick (10 Hz from a 500 Hz loop), matching the policy_dt in info.json.

Troubleshooting

Problem	Likely cause
No motion / topics silent	Stack not running or wrong network interface
Policy, PolicyParams not loaded	-p path wrong, info.json missing/invalid, or policy.onnx missing
Thread deadline … over!	System too loaded for 2 ms period — run with sudo (RT thread) or reduce load
apt authentication errors	Unrelated host GPG issue; fix separately

See also

• High Level Command
• C++ SDK Overview
• Concept — Low level / LV0