dejanb/hrobot-firmware

Hrobot firmware

ESP32-S3 firmware for Hrobot20dof230mm, a 230 mm, 654 g humanoid robot with 20 Dynamixel XL330 servos (five joints per leg without ankle roll, four per arm, two in the neck). The firmware runs policies learned with reinforcement learning (PPO in mjlab, MuJoCo physics) directly on the microcontroller, at 50 Hz, with only the measurements the robot has: the IMU and the servo encoders.

Videos of the same policies in MuJoCo physics: dejanb/hrobot-media.

Status. Both images build with ESP-IDF 5.4 and the policy code is verified on a PC against ONNX Runtime (below). They have not yet been run on a physical robot: the Dynamixel bus driver, the pin assignment, and the servo calibration are untested on hardware. Start with the robot suspended, so that it cannot fall.

Two firmware images

Folder	Image	Size	Policies
hrobot20dof230mm/basic/	hrobot20dof230mm.bin	896,016 bytes	walking on flat ground, getting up
hrobot20dof230mm/rough/	hrobot20dof230mm_rough.bin	1,200,176 bytes	walking on flat ground, getting up, walking on rough ground

Each folder also has bootloader.bin, partition-table.bin, flash_args (the flash offsets), and SHA256SUMS. The two images use different partition tables (the rough image needs a 1.5 MB app partition), so always flash all three files of the same folder.

Policies (each an MLP of 69 inputs, 256-128-64 hidden units with ELU, 20 outputs; about 60 thousand parameters, 236 kB in float32 or 63 kB in int8):

• walk: forward up to 0.25 m/s, backward up to 0.15 m/s, sideways up to 0.1 m/s, turning up to 0.8 rad/s; with a zero command the robot still steps in place.
• getup: rises from lying face down, face up, or on either side. The firmware switches to it when the trunk tilts more than 60° and back to walking after 1 s upright.
• rough (rough image only): a blind policy trained on bumps, slopes, and steps. In simulation it crosses 6 mm bumps and 10° slopes but stops at 6 mm steps. The rough image starts on this policy.

Hardware

• ESP32-S3 module with 8 MB flash (for example ESP32-S3-WROOM-1 N8R8).
• 20 x Dynamixel XL330-M288-T on one TTL bus through a half-duplex buffer (for example 74LVC2G241), powered at 5 V.
• ICM-42688-P IMU on SPI in the trunk.

Default pins (change them in source/esp32/main/main.cpp and rebuild if your board differs):

Signal	GPIO
servo bus TX / RX / direction	17 / 18 / 8
IMU SCLK / MOSI / MISO / CS	12 / 11 / 13 / 10

Servo setup (once, with ROBOTIS Dynamixel Wizard 2.0)

Set on every servo: ID as in the table below, baud rate 4 Mbps (register 8 = 6), return delay 0 (register 9), and operating mode 3 (position control).

ID	Joint	ID	Joint
1	left hip yaw	11	left shoulder pitch
2	left hip roll	12	left shoulder roll
3	left hip pitch	13	left elbow
4	left knee	14	left wrist
5	left ankle pitch	15	right shoulder pitch
6	right hip yaw	16	right shoulder roll
7	right hip roll	17	right elbow
8	right hip pitch	18	right wrist
9	right knee	19	neck yaw
10	right ankle pitch	20	neck pitch

The released images assume that encoder value 2048 is joint angle 0 and that every servo turns in the direction of its joint axis. On a real robot, measure the zero and the direction of each joint and enter them in source/src/robot_config.h (zero_ticks, sign), then rebuild.

Flashing the ESP32-S3

Option A: esptool only (no ESP-IDF needed). Install esptool (pip install esptool), connect the board over USB, and run in the folder of the image you want (basic/ shown; for rough/ replace the app file name):

python -m esptool --chip esp32s3 -b 460800 --before default_reset --after hard_reset \
  write_flash --flash_mode dio --flash_size 8MB --flash_freq 80m \
  0x0 bootloader.bin 0x8000 partition-table.bin 0x10000 hrobot20dof230mm.bin

On Linux the port is usually found automatically; otherwise add -p /dev/ttyACM0 (Windows: -p COM3).

Option B: from source with ESP-IDF 5.4, see below; idf.py -p /dev/ttyACM0 flash monitor builds, flashes, and opens the console.

Using it

Open the USB serial console of the ESP32-S3 (any serial terminal, or idf.py monitor). At start-up the firmware measures how long one policy step takes on the chip and prints it, for example walk policy float32: ... us per inference = ... inferences/s (needed: 50/s); every 5 s it prints the inference time and the length of the whole control cycle. Commands (one per line):

Command	Effect
v <vx> <vy> <wz>	velocity command: forward and sideways in m/s, turning in rad/s, e.g. v 0.2 0 0
v 0 0 0	stand (steps in place)
rough 1 / rough 0	rough-terrain policy on / off (rough image only; the basic image has no rough policy)
int8 1 / int8 0	int8 weights (faster) / float32 weights (default, exact copy of the trained network)

If fewer than 20 servos answer for three cycles in a row (60 ms), the firmware switches the torque off.

How the images are made from the ONNX policies

policies/*.onnx --gen_policy.py--> src/policy_*.h (C++ arrays) --ESP-IDF--> *.bin

1. Training. The policies are trained in mjlab and exported as ONNX (policies/walk.onnx, getup.onnx, rough.onnx); the observation normalizer is part of the graph.

2. ONNX to C++. source/gen_policy.py reads each ONNX file, checks its structure, and writes a self-contained header src/policy_<name>.h: the normalizer, every layer in float32 and in int8 (one scale per output row), the default joint pose, and the action scale. It also writes test vectors computed with ONNX Runtime. source/make_headers.sh does this for all three policies:

   pip install numpy onnx onnxruntime
   sh source/make_headers.sh
   

3. Check on a PC. test_host.cpp runs the C++ code on the test vectors:

   cd source/test && g++ -O2 -std=c++17 -I../src test_host.cpp -o test_host && ./test_host
   

   Result for the released policies: the float32 code matches ONNX Runtime to within 2e-6; int8 differs by up to 0.045 on actions of magnitude up to 4.6.

4. Build. With ESP-IDF 5.4 installed and exported (. $IDF_PATH/export.sh):

   cd source/esp32 && idf.py build        # -> build/hrobot20dof230mm.bin
   cd source/esp32_rough && idf.py build  # -> build/hrobot20dof230mm_rough.bin
   

   Both projects compile the same sources; the rough one adds HROBOT_WITH_ROUGH, which includes the rough policy and starts on it.

What runs on the chip

Every 20 ms (50 Hz, the rate used in training) the control task on core 1:

1. reads the present position and velocity of all 20 servos with one Dynamixel Protocol 2.0 SYNC_READ (dynamixel_bus.cpp);
2. takes the latest gyro rate and gravity direction from the IMU task, which runs at 1 kHz on core 0 (imu.cpp);
3. builds the 69-value observation exactly as in training: gyro (3), gravity (3), joint positions minus the default pose (20), joint velocities of the previous step (20), previous action (20), velocity command (3) (hrobot.cpp);
4. runs the network (hrobot_policy.hpp, plain loops, no libraries, no heap);
5. converts the actions to joint targets, default pose + action scale x action, clamps them to the joint ranges, and sends them with one SYNC_WRITE.

One bus cycle for 20 servos is about 530 bytes, or 1.3 ms at 4 Mbps.

Contents

hrobot20dof230mm/
  basic/      hrobot20dof230mm.bin, bootloader.bin, partition-table.bin, flash_args, SHA256SUMS
  rough/      hrobot20dof230mm_rough.bin, bootloader.bin, partition-table.bin, flash_args, SHA256SUMS
  policies/   walk.onnx, getup.onnx, rough.onnx
  source/     gen_policy.py, make_headers.sh, src/, test/, esp32/, esp32_rough/

License

CC BY 4.0, the same as dejanb/hrobot-media. Authors: Dejan Batanjac and Vladan Vučković.