+# Inspire RH56 dual-hand stack for the Unitree G1
-# 1. 📦 Introduction
+Runs the two Inspire RH56DFX hands on the G1's PC2 and puts a web dashboard in front of
+them: finger sliders, saved shapes, force-follow, live diagnostics, arm actions, and
+**arm + hand record / replay**.
-[Unitree Robot RH56DFX Inspire Hand](https://support.unitree.com/home/en/H1_developer/Dexterous_hand) Controller.
+Everything runs in **one Docker container on the robot**. The workstation is only where you
+edit the source and push it.
-
-
-
+Forked from Unitree's [DFX inspire hand service](https://support.unitree.com/home/en/H1_developer/Dexterous_hand);
+the service protocol section at the bottom is theirs.
+---
-The user sends `unitree_go::msg::dds::MotorCmds_` messages to the `rt/inspire/cmd` topic to control the dexterous hand,
-and receives `unitree_go::msg::dds::MotorStates_` messages from the `rt/inspire/state` topic to obtain its status.
+## Run it
+
+```bash
+./start.sh
+```
+
+That is the whole thing. It builds the image the first time (a few minutes), starts the
+container, waits until the dashboard actually answers, and prints the URL:
+
+```
+>> up
+ service=True bridge=True iface=eth0 ports=/dev/ttyUSB1, /dev/ttyUSB2
+ recorder: ready (record/replay panel enabled)
+ http://192.168.123.164:8088
+ http://10.255.254.88:8088
+```
+
+| command | does |
+|---|---|
+| `./start.sh` | start (builds the image if missing) |
+| `./start.sh stop` | stop the container |
+| `./start.sh restart` | stop + start |
+| `./start.sh status` | running? serving what? hands alive? |
+| `./start.sh logs` | follow the container log |
+| `./start.sh rebuild` | force an image rebuild, then start |
+
+**Run it on the robot.** Run it on the workstation and it rsyncs this folder to the robot
+and runs itself there — the hands, the DDS link to the arm and Docker are all on the robot,
+so there is nothing to run locally. Override the target with `ROBOT=unitree@10.255.254.88`.
+
+Only `./start.sh` is meant to be run by hand. `docker/start.sh` is the container's internal
+entrypoint; running it directly starts the services natively, outside Docker.
+
+---
+
+## What you get at `:8088`
+
+| card | what it does |
+|---|---|
+| **Fingers** | 12 sliders, presets, save/load named shapes |
+| **Force follow** | press a fingertip and that finger closes under your hand — the RH56 cannot be back-driven, so this admittance loop is the only way to shape it by hand |
+| **Joint tracker** | angle traces + per-finger force, current, force limit, temperature, ERROR/STATUS |
+| **Diagnostics** | live read-back and a close→open self-test, reported per hand |
+| **Read back** | copy the hand's current pose into the sliders |
+| **Arm + hand combo** | built-in arm actions fused with a hand pose ⚠ arm moves |
+| **Record / replay** | record the arm + hand, replay it, manage the library ⚠ arm moves |
+| **Library** | manage saved shapes and combos |
+
+---
+
+## Record / replay
+
+Runs [`recorder/g1_record_replay.py`](recorder/g1_record_replay.py) as a child process over a
+pty — the same script you would run from a terminal, so a take made in the browser and one
+made from the CLI are the same file in `DataG1/`.
+
+**A take:** arms hold for ~3 s, then go **limp** so you pose them by hand. Fingers *cannot*
+be posed by hand, so they are driven by the in-take keys or your saved shapes. At the end the
+recorder offers a preview (⚠ replays the take — the arm moves) and then a save; both are
+answered in the panel.
+
+**Stop** sends SIGINT, so the recorder walks the arm home and releases it. It does not kill.
+
+**Replay** has ⏸ Pause / ▶ Resume. Pause freezes the playback clock but keeps publishing the
+frame it stopped on — the arm holds the pose rather than dropping, because `rt/arm_sdk` needs
+a continuous command.
+
+### In-take keys — and when they do nothing
+
+`o` both to rest · `c` both closed · `[` `]` right open/close · `;` `'` left open/close ·
+`f` arm/disarm follow · `1`-`9` your saved shapes.
+
+Which of them the recorder acts on depends on `--fingers`. Measured against the real recorder
+while watching `rt/inspire/cmd`:
+
+| `--fingers` | what the keys do | records |
+|---|---|---|
+| `touch` *(default)* | **nothing** — the recorder holds the rest pose all take | how far a finger closes, from how hard you press it |
+| `position` | drive the hand for real | the finger's measured angle |
+| `external` | nothing — the recorder publishes no hand commands at all | the angle reached by the `:8088` sliders |
+| `follow` | only `f` and the shape keys reach the hand | the angle you push a finger into |
+
+The panel greys out keys the current mode ignores and says why, so a dead button is never a
+mystery. Note `[` and `;` also un-park thumb_rot, while `o` returns it to tucked.
+
+### The take library
+
+Per take: **▶ replay · ⬇ download · ⧉ duplicate · ✎ rename · 🗑 delete**, plus **⬆ Upload**
+(validated: the first line must be the recorder's own meta header) and **🗑 Delete all**.
+`arm_home.jsonl` is hidden and protected — it is the home pose every take returns through,
+not a recording.
+
+Takes live on the **host** at `~/hand_data/DataG1`, bind-mounted into the container.
+`docker/run.sh` begins with `docker rm -f`, so anything kept inside the image would be
+destroyed on every restart.
+
+---
+
+## Where things are
+
+| | |
+|---|---|
+| Workstation (canonical source) | `~/Robotics_workspace/yslootahtech/Project/G1/DFX_inspire_service` |
+| Robot | `~/DFX_inspire_service` |
+| In the container | `/opt/hand` (service + dashboard) · `/opt/recorder` (recorder + `DataG1`) |
+| Host data (survives rebuilds) | `~/hand_data` — `hand_poses.txt`, `hand_combos.json`, `DataG1/` |
+
+```
+inspire_g1 RS-485 -> both hands, publishes rt/inspire/state
+hand_bridge TCP 127.0.0.1:7799 -> rt/inspire/cmd, plus the force-follow loop at 30Hz
+hand_web.py the dashboard on :8088, and it spawns the recorder
+```
+
+All three run inside the single `inspire-hand` container (`--network host --privileged
+-v /dev:/dev` — DDS needs the host network, the CH340 adapters re-enumerate).
+
+### What is in the image
+
+Self-contained from a clean checkout: `recorder/` (the recorder + home pose),
+`vendor/unitree_sdk2_python/`, `web/`, `example/`, `include/`.
+
+The build still reads three things from the **robot's own system install**, because they are
+compiled libraries rather than source:
+
+- `~/unitree_sdk2` — the C++ static lib the binaries link against
+- `/usr/local/{include,lib}` — CycloneDDS headers and runtime
+- `~/cyclonedds/install/include/idl` — IDL headers; the Python `cyclonedds` wheel compiles a
+ `_idlpy` extension against them, and a normal CycloneDDS install does not put them in
+ `/usr/local`
+
+So: **runtime is 100% Docker; building the image needs the robot's SDK.** Override the
+locations with `UNITREE_SDK2_DIR`, `CYCLONEDDS_SRC`, `RECORDER_SRC`, `UNITREE_SDK2PY_DIR`.
+
+`web/` is copied in the last layer, so editing the dashboard rebuilds in seconds instead of
+recompiling every binary.
+
+---
+
+## Troubleshooting
+
+**`recorder: NOT found`** — the image was built without `recorder/` or `vendor/`. Rebuild:
+`./start.sh rebuild`.
+
+**Stop does nothing during a take** — the dashboard was started with SIGINT ignored (any
+process backgrounded by a non-interactive shell inherits `SIG_IGN`, and it survives `exec`).
+`hand_web.py` repairs this at startup; if the repair fails the panel says so. Restart it.
+
+**Fingers don't move (thumb might)** — usually a latched actuator fault from two `inspire_g1`
+running at once. `./start.sh restart` re-runs `ClearError`. Still stuck: power-cycle the
+hands (24 V).
+
+**"Port drift" warning** — the service opened different ttys than are live now; the CH340
+adapters re-enumerate on the hub. Restart.
+
+**A finger reads as dead (0 current, no movement)** — its force-sensor zero has drifted past
+the grip-force limit, so the RH56 refuses to drive it. `inspire_g1` measures the resting
+offset at startup and raises that hand's limit to clear it; the log names the fingers. The
+real fix is re-zeroing the sensor with the hand unloaded.
+
+**`rt/inspire/state` reads all zeros** — cosmetic; the CH340 adapters echo their own TX, so
+position read-back is unreliable. Commands are unaffected.
+
+**Two `inspire_g1` instances** — they collide on the RS-485 bus. `docker/run.sh` kills
+host-native copies before starting the container. Kill with `pkill -x inspire_g1`, never
+`pkill -f`.
+
+---
+
+## Native (non-Docker) path
+
+Kept for bring-up and debugging; `manage.sh` runs the services directly on the robot
+(`./manage.sh status|start|restart|dashboard|...`). It needs `./build.sh` first, since the
+compiled `build/` is not shipped. Do not run it while the container is up — two writers on
+the RS-485 bus corrupt the hands.
+
+Serial setup, udev rules and hand-side wiring: [SETUP_G1.md](SETUP_G1.md).
+Docker specifics: [docker/README.md](docker/README.md).
+
+---
+
+## Service protocol (upstream)
+
+Send `unitree_go::msg::dds::MotorCmds_` to `rt/inspire/cmd`; read
+`unitree_go::msg::dds::MotorStates_` from `rt/inspire/state`. Only `q` is meaningful —
+`1.00` = finger open, `0.00` = closed.
```mermaid
graph LR
@@ -23,101 +204,11 @@ A(user) --rt/inspire/cmd--> B(H1 or G1)
B --rt/inspire/state--> A
```
-The IDL data is an array containing joint-level values for all 12 motors of both hands.
-Currently the dexterous hand only supports joint control, i.e. only the parameter `q` makes sense in the idl format. The others are reserved.
+| id | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 |
+|---|---|---|---|---|---|---|---|---|---|---|---|---|
+| hand | right | right | right | right | right | right | left | left | left | left | left | left |
+| joint | pinky | ring | middle | index | thumb-bend | thumb-rot | pinky | ring | middle | index | thumb-bend | thumb-rot |
-
-
-
-
-
Id
-
0
-
1
-
2
-
3
-
4
-
5
-
6
-
7
-
8
-
9
-
10
-
11
-
-
-
Joint
-
Right Hand
-
Left Hand
-
-
-
pinky
-
ring
-
middle
-
index
-
thumb-bend
-
thumb-rotation
-
pinky
-
ring
-
middle
-
index
-
thumb-bend
-
thumb-rotation
-
-
-
-
-
-# 2. 🚀 Launch
-
-## unitree h1
-```bash
-sudo apt install libboost-all-dev libspdlog-dev
-# Build project
-mkdir build & cd build
-cmake ..
-make -j6
-# Terminal 1. Run h1 inspire hand service
-sudo ./inspire_h1 -s /dev/ttyUSB0
-# Terminal 2. Run example
-./hand_example
-```
-
-## unitree g1
-```bash
-sudo apt install libboost-all-dev libspdlog-dev
-# Build project
-mkdir build & cd build
-cmake ..
-make -j6
-# Terminal 1. Run g1 inspire hand service
-# The serial port name is hard-coded; if it doesn’t match your setup, please edit it directly in the source.
-sudo ./inspire_g1
-# Terminal 2. Run example
-./hand_example
-```
-
-# FAQ
-1. Error when `make -j6`
- ```bash
- ...
- /usr/bin/ld: inspire_ctrl.cpp:(.text._Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b[_Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b]+0x2be): undefined reference to `org::eclipse::cyclonedds::core::cdr::xcdr_v2_stream::finish_member(org::eclipse::cyclonedds::core::cdr::entity_properties&, bool)'
- /usr/bin/ld: inspire_ctrl.cpp:(.text._Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b[_Z14serialize_intoIN10unitree_go3msg4dds_12MotorStates_EN3org7eclipse10cyclonedds4core3cdr14xcdr_v2_streamEEbPvmRKT_b]+0x2d9): undefined reference to `org::eclipse::cyclonedds::core::cdr::xcdr_v2_stream::finish_struct(org::eclipse::cyclonedds::core::cdr::entity_properties&)'
-
- ```
- please compile and install `unitree_sdk2`:
- ```bash
- cd ~
- git clone https://github.com/unitreerobotics/unitree_sdk2
- cd unitree_sdk2
- mkdir build & cd build
- cmake ..
- sudo make install
- ```
-2. Error when run `sudo ./inspire_h1 -s /dev/ttyUSB0` or `sudo ./inspire_g1`
- ```bash
- --- Unitree Robotics ---
- Inspire Hand Controller
- Open serial port /dev/ttyUSB* failed
- ```
- For **Unitree h1**, use the `-s` parameter to change the serial port name.
- For **Unitree g1**, modify the serial port name directly in the source code.
+`hand_bridge` also serves this over TCP on `127.0.0.1:7799` (`S` = state+force, `R` = state,
+`F` = force-follow), which is how the dashboard talks to the hands without a DDS stack of its
+own.
diff --git a/SETUP_G1.md b/SETUP_G1.md
new file mode 100644
index 0000000..7a9f8ec
--- /dev/null
+++ b/SETUP_G1.md
@@ -0,0 +1,104 @@
+# Inspire RH56 dual-hand bring-up on Unitree G1 (PC2)
+
+Setup notes specific to **this** robot. Upstream README assumes a clean
+`unitree_sdk2` install in `/usr/local` and hard-codes `/dev/ttyUSB1`+`/dev/ttyUSB2`;
+neither holds here, so we adjust as below.
+
+Canonical (workstation): `…/yslootahtech/Project/G1/DFX_inspire_service`
+Deploy to PC2 `unitree@192.168.123.164:~/DFX_inspire_service` via `rsync`, then `./build.sh`.
+(wifi `10.255.254.86` is flaky; the eth0 IP `192.168.123.164` is the reliable path.)
+
+## 0. Easiest way to drive everything — `manage.sh`
+```bash
+./manage.sh # status: service + devices + adapters
+./manage.sh start|stop|restart
+./manage.sh open|close|box|menu # fingers only
+./manage.sh like|handshake|thumbup # arm + hand (ARM MOVES)
+./manage.sh arm "shake hand" # built-in arm action; armlist to list
+./manage.sh help
+```
+It keeps exactly **one** `inspire_g1` running (duplicates corrupt the RS-485 bus) and
+auto-starts the service for gesture commands.
+
+## Architecture (unchanged from upstream)
+`hand_example`/your app → DDS topic `rt/inspire/cmd` (`MotorCmds_`, 12 motors:
+0–5 right, 6–11 left) → **`inspire_g1`** → RS-485 (CH340 USB adapters, 115200 8N1,
+Inspire slave id 1) → hands. State flows back on `rt/inspire/state`.
+Finger order per hand: pinky, ring, middle, index, thumb-bend, thumb-rotation.
+`q` ∈ [0,1]: **0 = closed, 1 = open**.
+
+## 1. Build (no system changes)
+`unitree_sdk2` in `/usr/local` is stale (no go2 IDL). `build.sh` links against the
+newer `~/unitree_sdk2` source tree instead:
+```bash
+cd ~/DFX_inspire_service
+./build.sh # -> build/inspire_g1, inspire_h1, hand_example
+```
+
+## 2. Stable serial names + no-sudo access (one-time, needs sudo)
+Both hand adapters are CH340s with identical USB serial, so we pin names by
+physical port and grant access via udev:
+```bash
+sudo cp ~/DFX_inspire_service/udev/99-inspire-hands.rules /etc/udev/rules.d/
+sudo udevadm control --reload-rules && sudo udevadm trigger
+ls -l /dev/inspire_right /dev/inspire_left # both should now exist
+```
+`inspire_g1.cpp` opens `/dev/inspire_right` + `/dev/inspire_left`.
+Mapping (corrected after live test): right → USB port `1-2.2.1.1`, left → `1-2.2.3`.
+
+## 3. Run + test
+```bash
+cd ~/DFX_inspire_service/build
+./inspire_g1 # Terminal 1 (no sudo needed after the udev rule)
+./hand_example # Terminal 2 -> both hands open/close ~1 Hz
+```
+Stop with Ctrl+C in each. If a hand doesn't move: check 24 V power, RS-485 A/B
+polarity, and that the hand is at Inspire slave id 1.
+
+## Record / replay panel (dashboard)
+
+The dashboard's **Record / replay** card runs `g1_record_replay.py` as a child process — the
+same script as the CLI, so a take made in the browser and one made from a terminal are the
+same file in `DataG1/`. It needs the recorder present on whatever host serves the dashboard:
+
+```bash
+# where g1_record_replay.py lives (searched: ../Manual_Recorder, ~/Manual_Recorder)
+export RECORDER_DIR=~/Manual_Recorder
+# an interpreter that can import unitree_sdk2py (on a workstation: the g1_env conda python)
+export RECORDER_PY=python3
+./manage.sh dashboard # prints the resolved recorder path, or NOT FOUND
+```
+
+The panel starts a take, relays the in-take keys (`o c [ ] ; '`, `f`, and the saved shapes as
+keys 1-9), answers the recorder's preview/save prompts, and lists `DataG1/` for replay,
+rename and delete. Two things it works around, both load-bearing:
+
+- **Keys are paced ~150 ms apart.** The recorder reads them with `select()` + buffered
+ `sys.stdin.read(1)`; a burst lands in Python's buffer, `select()` then reports nothing
+ ready and every key after the first is stranded. Measured: 3 keys 10 ms apart → 2 arrived.
+- **Stop sends SIGINT, and the dashboard repairs SIGINT first.** A process backgrounded by a
+ non-interactive shell inherits `SIGINT = SIG_IGN`, which survives `exec` — and `manage.sh`
+ starts the dashboard exactly that way, so without the repair Stop would silently do
+ nothing. The panel shows a warning if the repair did not hold. SIGINT (not SIGKILL) is
+ what lets the recorder walk the arm home and release it.
+
+Replay and preview **move the arm** and both require an explicit confirmation. Starting a
+take disarms force-follow, since the bridge and the recorder would otherwise both publish
+`rt/inspire/cmd` every frame.
+
+## If you move an adapter to a different USB socket
+The udev `KERNELS` value changes. Re-discover and update the rule:
+```bash
+udevadm info -a -n /dev/ttyUSB0 | grep -m1 'KERNELS=="[0-9]'
+```
+
+## Troubleshooting
+- **`/dev/inspire_*` missing / no CH340 in `lsusb`** → the hand USB-RS485 adapters are
+ unplugged (or robot rebooted with them out). Re-plug them; udev recreates the names.
+- **Fingers don't move (thumb might)** → usually a latched actuator fault from running
+ two `inspire_g1` at once. `inspire_g1` now runs `ClearError` + sets speed/force on
+ startup; `./manage.sh restart` re-applies it. If still stuck, power-cycle the hands (24 V).
+- **Two `inspire_g1` instances** → they collide on the serial bus; `./manage.sh status`
+ flags it, `restart` fixes it. Kill with `pkill -x inspire_g1` (never `pkill -f`).
+- **`rt/inspire/state` reads all-zeros** → cosmetic; CH340 adapters echo their TX so
+ position read-back is unreliable. Does not affect commands sent to the hand.
diff --git a/build.sh b/build.sh
new file mode 100755
index 0000000..ee1f1c0
--- /dev/null
+++ b/build.sh
@@ -0,0 +1,29 @@
+#!/usr/bin/env bash
+# Build DFX_inspire_service on the G1 onboard PC (PC2 / Jetson, aarch64).
+#
+# Why the extra flags: the factory unitree_sdk2 installed in /usr/local is stale
+# and lacks the go2 IDL headers (MotorCmds_/MotorStates_) + matching symbols.
+# We point the compiler/linker at the newer unitree_sdk2 source tree instead,
+# without touching /usr/local (no `sudo make install`).
+#
+# Override the SDK location with: UNITREE_SDK2_DIR=/path/to/unitree_sdk2 ./build.sh
+set -euo pipefail
+
+SDK="${UNITREE_SDK2_DIR:-$HOME/unitree_sdk2}"
+ARCH="$(uname -m)" # aarch64 on the G1 Jetson
+
+if [ ! -f "${SDK}/lib/${ARCH}/libunitree_sdk2.a" ]; then
+ echo "ERROR: unitree_sdk2 not found at ${SDK} (expected ${SDK}/lib/${ARCH}/libunitree_sdk2.a)" >&2
+ echo " Set UNITREE_SDK2_DIR to the unitree_sdk2 source tree." >&2
+ exit 1
+fi
+
+cd "$(dirname "$0")"
+mkdir -p build && cd build
+cmake .. \
+ -DCMAKE_CXX_FLAGS="-I${SDK}/include" \
+ -DCMAKE_EXE_LINKER_FLAGS="-L${SDK}/lib/${ARCH}"
+make -j"$(nproc)"
+
+echo
+echo "Built: $(pwd)/inspire_g1 inspire_h1 hand_example"
diff --git a/docker/Dockerfile b/docker/Dockerfile
new file mode 100644
index 0000000..75b0c95
--- /dev/null
+++ b/docker/Dockerfile
@@ -0,0 +1,70 @@
+# Inspire hand service + web dashboard, self-contained (aarch64 / Jetson).
+# Build context is assembled by build_image.sh (pulls unitree_sdk2 + CycloneDDS
+# from the robot). Run with: --network host --privileged -v /dev:/dev
+FROM arm64v8/ubuntu:20.04
+
+ENV DEBIAN_FRONTEND=noninteractive TZ=UTC
+RUN apt-get update && apt-get install -y --no-install-recommends \
+ build-essential cmake \
+ libboost-all-dev libspdlog-dev libeigen3-dev \
+ python3 python3-flask \
+ iproute2 procps ca-certificates \
+ && rm -rf /var/lib/apt/lists/*
+
+# CycloneDDS (ddscxx + iceoryx) headers & runtime libs from the robot's /usr/local
+COPY deps/usr_local_include/ /usr/local/include/
+COPY deps/usr_local_lib/ /usr/local/lib/
+RUN ldconfig
+
+# unitree_sdk2 headers + prebuilt static lib (for build.sh linking)
+COPY deps/unitree_sdk2/ /opt/unitree_sdk2/
+
+# Record/replay panel: the recorder is a python child process that speaks DDS itself, so
+# the image needs the bindings as well as the script. Skipped cleanly when build_image.sh
+# could not find the sources (empty dirs) — the panel then reports "recorder not found".
+# NOTE: this layer pip-installs, so the robot needs network access at BUILD time. Offline
+# alternative: copy the robot's own site-packages in the same way the C libs are copied.
+COPY deps/recorder/ /opt/recorder/
+COPY deps/unitree_sdk2_python/ /opt/unitree_sdk2_python/
+# cyclonedds is pinned to what unitree_sdk2py demands (0.10.2) — installing anything else
+# just gets downgraded when the SDK lands. It is built from sdist here, so it compiles
+# against the robot's own CycloneDDS headers (0.10.5) and links its libddsc.
+ARG CYCLONEDDS_PY=0.10.2
+ENV CYCLONEDDS_HOME=/usr/local
+# The SDK is COPIED into dist-packages rather than pip-installed, and both alternatives
+# were tried on the robot first:
+# * a wheel install loses four subpackages — b2, comm, g1, h1 ship with no __init__.py,
+# so find_packages() drops them and the package's own
+# `from . import idl, utils, core, rpc, go2, b2` dies with "cannot import name 'b2'";
+# * `pip install -e` reports success (runs setup.py develop) but never lands on sys.path
+# in this image — the very next line still gets "No module named 'unitree_sdk2py'".
+# A copy is deterministic and keeps every subdirectory, including the ones that resolve as
+# implicit namespace packages. numpy comes from apt; opencv-python (its other declared
+# dependency, ~100MB+) is skipped because nothing in the SDK imports cv2 — verified.
+# The destination is derived from the freshly-installed cyclonedds rather than from
+# sysconfig: on Debian/Ubuntu sysconfig's purelib is /usr/lib/python3.8/site-packages,
+# which is not even on sys.path (it is dist-packages here), so the copy landed nowhere.
+RUN if [ -d /opt/unitree_sdk2_python/unitree_sdk2py ]; then \
+ apt-get update && apt-get install -y --no-install-recommends \
+ python3-pip python3-dev python3-numpy && \
+ pip3 install --no-cache-dir "cyclonedds==${CYCLONEDDS_PY}" && \
+ cp -r /opt/unitree_sdk2_python/unitree_sdk2py \
+ "$(python3 -c 'import cyclonedds,os;print(os.path.dirname(os.path.dirname(cyclonedds.__file__)))')/" && \
+ python3 -c "import unitree_sdk2py, cyclonedds; print('python DDS ok')" && \
+ rm -rf /var/lib/apt/lists/* ; \
+ else echo "no unitree_sdk2_python in context — record/replay disabled"; fi
+ENV RECORDER_DIR=/opt/recorder RECORDER_PY=python3
+
+# the service sources (everything except web/), then build the binaries in-image
+COPY app/ /opt/hand/
+WORKDIR /opt/hand
+RUN chmod +x build.sh docker/start.sh manage.sh 2>/dev/null; \
+ UNITREE_SDK2_DIR=/opt/unitree_sdk2 ./build.sh
+
+# The dashboard goes in LAST, on purpose: it is the part that changes every other day, and
+# from here a redeploy is a seconds-long layer copy instead of a full C++ recompile.
+COPY app_web/ /opt/hand/web/
+
+ENV HAND_REPO=/opt/hand
+EXPOSE 8088
+CMD ["/opt/hand/docker/start.sh"]
diff --git a/docker/README.md b/docker/README.md
new file mode 100644
index 0000000..bed46c7
--- /dev/null
+++ b/docker/README.md
@@ -0,0 +1,57 @@
+# Dockerized Inspire hand service + dashboard
+
+One self-contained image (aarch64) running `inspire_g1` + `hand_bridge` + the Flask
+dashboard. Build ON the robot (it pulls `unitree_sdk2` + CycloneDDS from the host).
+
+## Build (once, on the robot)
+```bash
+cd ~/DFX_inspire_service
+./docker/build_image.sh # assembles context + docker build -> inspire-hand:latest
+```
+
+## Run
+```bash
+./docker/run.sh # or: docker compose -f docker/docker-compose.yml up -d
+# dashboard -> http://:8088
+./docker/stop.sh
+```
+
+`run.sh` first stops any host-native `inspire_g1`/`hand_bridge` — running both the
+container and the host service would put **two writers on the RS-485 bus** and
+corrupt the hands.
+
+## Why these flags
+- `--network host` — CycloneDDS (rt/inspire/cmd) and the arm actions (rt/arm_sdk,
+ reaching the G1 motion controller on eth0) need the host network.
+- `--privileged -v /dev:/dev` — the CH340 `ttyUSB*` adapters re-enumerate on the hub;
+ mounting `/dev` + privileged lets the in-container dynamic detection find them.
+- `IFACE=eth0` — interface for arm actions (must be eth0). Fingers work on either.
+
+## Record / replay in the image
+The dashboard's Record/Replay card runs `g1_record_replay.py` as a child process, so the
+image needs the script **and** a python that can import `unitree_sdk2py`. `build_image.sh`
+pulls both into the context:
+
+```bash
+RECORDER_SRC=~/Manual_Recorder \
+UNITREE_SDK2PY_DIR=~/unitree_sdk2_python \
+ ./docker/build_image.sh
+```
+
+Neither is required — leave them out and the image still builds, with the panel reporting
+*"recorder not found"*. `start.sh` prints which case you got.
+
+- **That layer pip-installs (`cyclonedds`, `unitree_sdk2py`), so the robot needs network at
+ BUILD time.** Offline alternative: copy the robot's own python site-packages into the
+ image the same way `build_image.sh` copies the C libs.
+- **Takes are a host volume** — `run.sh` mounts `$HAND_DATA_DIR/DataG1` to
+ `/opt/recorder/DataG1`, because `run.sh` starts with `docker rm -f` and would otherwise
+ destroy every recording ever made.
+- **`arm_home.jsonl` is seeded on first start**, not baked into `DataG1/` — the bind-mount
+ would hide a baked copy, and without it every take skips its return-to-home. It is hidden
+ from the takes list and cannot be deleted from the panel: it is a home pose, not a take.
+
+## Notes
+- Sudo/udev not needed inside the container (privileged).
+- Saved poses/combos persist via `-v $HAND_DATA_DIR:/opt/hand/data` + `HAND_DATA` (set by
+ `run.sh`). They used to live in the image and were wiped by every rebuild.
diff --git a/docker/build_image.sh b/docker/build_image.sh
new file mode 100755
index 0000000..9cf6234
--- /dev/null
+++ b/docker/build_image.sh
@@ -0,0 +1,80 @@
+#!/usr/bin/env bash
+# Assemble the build context (pull unitree_sdk2 + CycloneDDS from this robot) and
+# build the self-contained image. Run ON the robot (aarch64).
+# ./docker/build_image.sh
+set -euo pipefail
+
+HERE="$(cd "$(dirname "$0")" && pwd)" # .../DFX_inspire_service/docker
+REPO="$(dirname "$HERE")" # .../DFX_inspire_service
+SDK="${UNITREE_SDK2_DIR:-$HOME/unitree_sdk2}"
+CTX="$HERE/.ctx"
+IMG="${IMG:-inspire-hand:latest}"
+
+[ -f "$SDK/lib/aarch64/libunitree_sdk2.a" ] || { echo "ERROR: unitree_sdk2 not at $SDK"; exit 1; }
+
+echo ">> assembling build context in $CTX"
+rm -rf "$CTX"
+mkdir -p "$CTX/deps/usr_local_include" "$CTX/deps/usr_local_lib" "$CTX/deps/unitree_sdk2" "$CTX/app"
+
+# unitree_sdk2 headers + static lib
+cp -r "$SDK/include" "$CTX/deps/unitree_sdk2/"
+cp -r "$SDK/lib" "$CTX/deps/unitree_sdk2/"
+
+# CycloneDDS headers: C (dds, ddsc), C++ (ddscxx) and iceoryx
+for d in dds ddsc ddscxx iceoryx; do cp -r "/usr/local/include/$d" "$CTX/deps/usr_local_include/"; done
+
+# The IDL headers (idl/, idlc/) are needed ONLY by the python binding: pip building
+# cyclonedds compiles a _idlpy extension against idl/retcode.h, and `make install` of the
+# C++ side does not put them in /usr/local/include. Without them the image build dies at
+# idlpy/src/context.h:17: fatal error: idl/retcode.h: No such file or directory
+# CYCLONEDDS_SRC points at the checkout whose install/ prefix has them (0.10.5 here).
+CDDS_SRC="${CYCLONEDDS_SRC:-$HOME/cyclonedds}"
+for d in idl idlc; do
+ if [ -d "/usr/local/include/$d" ]; then cp -r "/usr/local/include/$d" "$CTX/deps/usr_local_include/"
+ elif [ -d "$CDDS_SRC/install/include/$d" ]; then cp -r "$CDDS_SRC/install/include/$d" "$CTX/deps/usr_local_include/"
+ fi
+done
+# CycloneDDS shared runtime libs
+cp -P /usr/local/lib/libddsc* /usr/local/lib/libddscxx* \
+ /usr/local/lib/libcyclonedds* /usr/local/lib/libdds_security_* \
+ "$CTX/deps/usr_local_lib/" 2>/dev/null || true
+
+# The app, with web/ split into its own context dir. The Dockerfile copies web/ AFTER the
+# C++ build, so editing the dashboard rebuilds in seconds instead of recompiling every
+# binary — and the dashboard is what changes most often.
+rsync -a --exclude 'build/' --exclude 'docker/.ctx/' --exclude 'web/' "$REPO/" "$CTX/app/"
+rsync -a "$REPO/web/" "$CTX/app_web/"
+cp "$HERE/Dockerfile" "$CTX/Dockerfile"
+
+# ---- recorder for the dashboard's Record/Replay panel ---------------------------------
+# The panel runs g1_record_replay.py as a CHILD PROCESS, so the script and a python that
+# can import unitree_sdk2py both have to be inside the image. Both dirs are always created
+# (empty is fine) because the Dockerfile COPYs them unconditionally — with an empty one the
+# image still builds and the panel simply reports "recorder not found".
+# Both live IN this project (recorder/ and vendor/), so a clean checkout builds a complete
+# image with no environment set and nothing fetched from elsewhere on the machine. The env
+# vars remain as overrides for anyone pointing at their own working copy.
+REC="${RECORDER_SRC:-$REPO/recorder}"
+SDKPY="${UNITREE_SDK2PY_DIR:-$REPO/vendor/unitree_sdk2_python}"
+mkdir -p "$CTX/deps/recorder/seed" "$CTX/deps/unitree_sdk2_python"
+if [ -f "$REC/g1_record_replay.py" ] && [ -f "$SDKPY/setup.py" ]; then
+ cp "$REC/g1_record_replay.py" "$CTX/deps/recorder/"
+ # The home pose is seeded rather than shipped in DataG1/, because run.sh bind-mounts
+ # DataG1 over the top and would hide anything baked there.
+ for h in "$REC/arm_home.jsonl" "$REC/DataG1/arm_home.jsonl"; do
+ [ -f "$h" ] && { cp "$h" "$CTX/deps/recorder/seed/"; break; }
+ done
+ rsync -a --exclude '.git/' --exclude '*.egg-info/' "$SDKPY/" "$CTX/deps/unitree_sdk2_python/"
+ echo ">> recorder included: $REC + unitree_sdk2py: $SDKPY"
+else
+ echo ">> NOTE: record/replay panel will be DISABLED in this image."
+ [ -f "$REC/g1_record_replay.py" ] || echo " missing $REC/g1_record_replay.py (set RECORDER_SRC)"
+ [ -f "$SDKPY/setup.py" ] || echo " missing $SDKPY/setup.py (set UNITREE_SDK2PY_DIR)"
+fi
+
+echo ">> docker build -t $IMG (this compiles the binaries in-image; takes a few min)"
+cd "$CTX"
+docker build -t "$IMG" .
+rm -rf "$CTX"
+echo ">> done: $IMG"
+echo " run with: $REPO/docker/run.sh"
diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml
new file mode 100644
index 0000000..f60c5e3
--- /dev/null
+++ b/docker/docker-compose.yml
@@ -0,0 +1,18 @@
+# Run the pre-built image (build first: ./docker/build_image.sh).
+# docker compose -f docker/docker-compose.yml up -d
+services:
+ inspire-hand:
+ image: inspire-hand:latest
+ container_name: inspire-hand
+ network_mode: host # CycloneDDS + arm actions need the host network (eth0)
+ privileged: true # + /dev mount so the container sees the CH340 ttyUSB*
+ volumes:
+ - /dev:/dev
+ # Compose does NOT expand a bare ~ in a volume path (it would create a directory
+ # literally named "~"), so the default goes through $HOME.
+ - ${HAND_DATA_DIR:-${HOME}/hand_data}:/opt/hand/data # shapes + combos
+ - ${HAND_DATA_DIR:-${HOME}/hand_data}/DataG1:/opt/recorder/DataG1 # takes
+ environment:
+ - IFACE=eth0
+ - HAND_DATA=/opt/hand/data
+ restart: unless-stopped
diff --git a/docker/run.sh b/docker/run.sh
new file mode 100755
index 0000000..cea5380
--- /dev/null
+++ b/docker/run.sh
@@ -0,0 +1,42 @@
+#!/usr/bin/env bash
+# Run the containerized hand stack (service + bridge + dashboard).
+# Stops any host-native inspire_g1/bridge first — two services would collide on
+# the RS-485 bus. Needs --network host (DDS) + --privileged -v /dev:/dev (serial).
+set -e
+IMG="${IMG:-inspire-hand:latest}"
+IFACE="${IFACE:-eth0}"
+
+echo ">> removing old container, then killing host-native services (avoid double inspire_g1)"
+docker rm -f inspire-hand 2>/dev/null || true # kill container's procs first
+pkill -x inspire_g1 2>/dev/null || true # now only host natives remain
+pkill -x hand_bridge 2>/dev/null || true
+pkill -f hand_web.py 2>/dev/null || true
+sleep 1; pkill -9 -x inspire_g1 2>/dev/null || true
+
+# INSPIRE_RIGHT_PATH pins which physical USB socket is the RIGHT hand. Both hands answer on
+# RS-485 id 1, so without this the side is inferred from USB path ORDER — correct today, but
+# it would flip undetected if the two dongles were swapped between sockets.
+# Operator-verified at the robot 2026-08-03: usb 1-2.2.1.1 is the physical RIGHT hand.
+# (An earlier wiggle test concluded the opposite; the operator watching the robot corrected
+# it. Flip this one value if the sides ever read swapped again — nothing else needs changing.)
+RIGHT_PATH="${INSPIRE_RIGHT_PATH:-1-2.2.1.1}"
+# Saved shapes/combos on the HOST, so rebuilding the image no longer wipes the library.
+DATA_DIR="${HAND_DATA_DIR:-$HOME/hand_data}"
+# Recordings live on the HOST for the same reason the shape library does: a `docker rm -f`
+# in this script would otherwise destroy every take ever made.
+mkdir -p "$DATA_DIR" "$DATA_DIR/DataG1"
+
+docker run -d --name inspire-hand --restart unless-stopped \
+ --network host --privileged -v /dev:/dev \
+ -v "$DATA_DIR":/opt/hand/data \
+ -v "$DATA_DIR/DataG1":/opt/recorder/DataG1 \
+ -e HAND_DATA=/opt/hand/data \
+ -e INSPIRE_RIGHT_PATH="$RIGHT_PATH" \
+ -e IFACE="$IFACE" "$IMG"
+echo ">> right hand pinned to usb $RIGHT_PATH · data dir $DATA_DIR · takes $DATA_DIR/DataG1"
+
+sleep 4
+docker logs --tail 24 inspire-hand || true
+echo ">> inspire_g1 instances (want 1, the container's): $(pgrep -xc inspire_g1)"
+ip=$(hostname -I | awk '{print $1}')
+echo ">> dashboard: http://$ip:8088"
diff --git a/docker/start.sh b/docker/start.sh
new file mode 100755
index 0000000..a3d307e
--- /dev/null
+++ b/docker/start.sh
@@ -0,0 +1,44 @@
+#!/usr/bin/env bash
+# Container entrypoint: supervised inspire_g1 + hand_bridge + Flask dashboard.
+set -u
+cd /opt/hand
+export HAND_REPO=/opt/hand
+IFACE="${IFACE:-eth0}"
+echo "[inspire-hand] iface=$IFACE ttyUSB: $(ls /dev/ttyUSB* 2>/dev/null | tr '\n' ' ')"
+
+# Supervisor: (re)start inspire_g1 whenever the CH340 adapters are present but the
+# service isn't running. Self-heals across the flaky-hub disconnect/reconnect cycles.
+(
+ while true; do
+ if ! pgrep -x inspire_g1 >/dev/null 2>&1; then
+ if ls /dev/ttyUSB* >/dev/null 2>&1; then
+ echo "[supervisor] adapters present, starting inspire_g1: $(ls /dev/ttyUSB* | tr '\n' ' ')"
+ ./build/inspire_g1 >/tmp/inspire_g1.log 2>&1 &
+ sleep 2
+ sed 's/^/[inspire_g1] /' /tmp/inspire_g1.log
+ fi
+ fi
+ sleep 3
+ done
+) &
+
+# TCP->DDS bridge for the dashboard (stays up regardless; commands no-op if service down)
+setsid ./build/hand_bridge "$IFACE" 7799 >/tmp/hand_bridge.log 2>&1 /dev/null | grep -vc arm_home) takes)"
+else
+ echo "[inspire-hand] recorder: NOT in image — record/replay panel disabled"
+fi
+
+echo "[inspire-hand] dashboard on :8088"
+exec python3 web/hand_web.py --port 8088
diff --git a/docker/stop.sh b/docker/stop.sh
new file mode 100755
index 0000000..b76d37f
--- /dev/null
+++ b/docker/stop.sh
@@ -0,0 +1,2 @@
+#!/usr/bin/env bash
+docker rm -f inspire-hand 2>/dev/null && echo "inspire-hand stopped" || echo "inspire-hand not running"
diff --git a/example/arm_action.cpp b/example/arm_action.cpp
new file mode 100644
index 0000000..bb5f83e
--- /dev/null
+++ b/example/arm_action.cpp
@@ -0,0 +1,91 @@
+/**
+ * @file arm_action.cpp
+ * @brief Trigger Unitree's built-in G1 upper-body actions (shake hand, etc.).
+ *
+ * Uses unitree_sdk2 G1ArmActionClient. The action server drives rt/arm_sdk for
+ * you (balance-aware, Unitree-designed motions) — do NOT run arm_raise at the
+ * same time (error 7400 = rt/arm_sdk occupied).
+ *
+ * ./arm_action list # list actions
+ * ./arm_action "shake hand" # by name
+ * ./arm_action 27 [iface] # by id (default iface: eth0)
+ * ./arm_action "release arm" # = 99, returns the arm to normal
+ *
+ * After most actions the arm HOLDS until you send "release arm" (99).
+ * Actions require the robot FSM id in {500,501,801} (check rt/sportmodestate).
+ */
+
+#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
+#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
+
+#include
+#include
+#include
+
+using namespace unitree::robot::g1;
+
+int main(int argc, char **argv)
+{
+ if (argc < 2) {
+ std::cout << "Usage: ./arm_action [networkInterface]\n";
+ return 1;
+ }
+ std::string action = argv[1];
+ std::string iface = argc > 2 ? argv[2] : "eth0";
+
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+ auto client = std::make_shared();
+ client->Init();
+ client->SetTimeout(10.f);
+
+ if (action == "list") {
+ std::cout << "Built-in actions (id name):\n";
+ for (const auto &kv : client->action_map)
+ std::cout << " " << kv.second << "\t" << kv.first << "\n";
+ std::string data;
+ if (client->GetActionList(data) == 0)
+ std::cout << "\nServer-reported list:\n" << data << std::endl;
+ return 0;
+ }
+
+ // Resolve action id from a number or a name.
+ int32_t id = 0;
+ try {
+ id = std::stoi(action);
+ } catch (const std::exception &) {
+ auto it = client->action_map.find(action);
+ if (it == client->action_map.end()) {
+ std::cerr << "Unknown action '" << action << "'. Try: ./arm_action list\n";
+ return 1;
+ }
+ id = it->second;
+ }
+
+ std::cout << "Executing action id " << id << " ..." << std::endl;
+ int32_t ret = client->ExecuteAction(id);
+ if (ret == 0) {
+ std::cout << "OK. (Arm holds until you send \"release arm\" / 99.)" << std::endl;
+ return 0;
+ }
+
+ switch (ret) {
+ case UT_ROBOT_ARM_ACTION_ERR_ARMSDK:
+ std::cerr << "Error 7400: " << UT_ROBOT_ARM_ACTION_ERR_ARMSDK_DESC
+ << " (something else holds rt/arm_sdk — e.g. arm_raise)." << std::endl;
+ break;
+ case UT_ROBOT_ARM_ACTION_ERR_HOLDING:
+ std::cerr << "Error 7401: " << UT_ROBOT_ARM_ACTION_ERR_HOLDING_DESC << std::endl;
+ break;
+ case UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID:
+ std::cerr << "Error 7402: " << UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID_DESC << std::endl;
+ break;
+ case UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID:
+ std::cerr << "Error 7404: invalid FSM id. Actions need fsm id {500,501,801} "
+ "(check rt/sportmodestate; in 801 only fsm mode {0,3})." << std::endl;
+ break;
+ default:
+ std::cerr << "Execute action failed, error code: " << ret << std::endl;
+ break;
+ }
+ return ret;
+}
diff --git a/example/arm_raise.cpp b/example/arm_raise.cpp
new file mode 100644
index 0000000..e9dcccf
--- /dev/null
+++ b/example/arm_raise.cpp
@@ -0,0 +1,170 @@
+/**
+ * @file arm_raise.cpp
+ * @brief SAFE standalone test: raise the G1 RIGHT arm via the arm_sdk interface.
+ *
+ * Based on unitree_sdk2 example g1_arm7_sdk_dds_example.cpp, hardened for safety:
+ * - aborts if rt/lowstate is not received (never commands from bogus zeros)
+ * - ENTER-gated phases (engage -> raise -> lower)
+ * - Ctrl+C ramps the blend weight to 0 (gracefully hands the arm back)
+ *
+ * Moves ONLY the right arm; left arm + waist are held at their measured pose.
+ *
+ * ./arm_raise [networkInterface] (default: eth0)
+ *
+ * !!! THE ARM PHYSICALLY MOVES. Clear the space, stop any other arm controller,
+ * keep the E-stop in hand. !!!
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+static const std::string kTopicArmSDK = "rt/arm_sdk";
+static const std::string kTopicState = "rt/lowstate";
+
+enum JointIndex {
+ kLeftHipPitch, kLeftHipRoll, kLeftHipYaw, kLeftKnee, kLeftAnkle, kLeftAnkleRoll,
+ kRightHipPitch, kRightHipRoll, kRightHipYaw, kRightKnee, kRightAnkle, kRightAnkleRoll,
+ kWaistYaw, kWaistRoll, kWaistPitch,
+ kLeftShoulderPitch, kLeftShoulderRoll, kLeftShoulderYaw, kLeftElbow,
+ kLeftWristRoll, kLeftWristPitch, kLeftWristYaw,
+ kRightShoulderPitch, kRightShoulderRoll, kRightShoulderYaw, kRightElbow,
+ kRightWristRoll, kRightWristPitch, kRightWristYaw,
+ kNotUsedJoint, kNotUsedJoint1, kNotUsedJoint2, kNotUsedJoint3, kNotUsedJoint4, kNotUsedJoint5
+};
+
+static std::atomic g_stop{false};
+static void onSigint(int) { g_stop = true; }
+
+int main(int argc, char **argv)
+{
+ const std::string iface = argc > 1 ? argv[1] : "eth0";
+ std::signal(SIGINT, onSigint);
+
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+
+ unitree::robot::ChannelPublisherPtr pub;
+ pub.reset(new unitree::robot::ChannelPublisher(kTopicArmSDK));
+ pub->InitChannel();
+
+ static unitree_hg::msg::dds_::LowState_ state;
+ std::atomic got{false};
+ unitree::robot::ChannelSubscriberPtr sub;
+ sub.reset(new unitree::robot::ChannelSubscriber(kTopicState));
+ sub->InitChannel([&](const void *m) {
+ std::memcpy(&state, m, sizeof(state));
+ got = true;
+ }, 1);
+
+ // SAFETY 1: require fresh state before touching the arm.
+ std::cout << "Waiting for " << kTopicState << " on '" << iface << "' ..." << std::endl;
+ for (int i = 0; i < 300 && !got; i++) std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ if (!got) {
+ std::cerr << "ERROR: no " << kTopicState << " received. Is the robot controller up, and is '"
+ << iface << "' the interface to the robot? Aborting (no arm command sent)." << std::endl;
+ return 1;
+ }
+
+ const std::array arm = {
+ kLeftShoulderPitch, kLeftShoulderRoll, kLeftShoulderYaw, kLeftElbow,
+ kLeftWristRoll, kLeftWristPitch, kLeftWristYaw,
+ kRightShoulderPitch, kRightShoulderRoll, kRightShoulderYaw, kRightElbow,
+ kRightWristRoll, kRightWristPitch, kRightWristYaw,
+ kWaistYaw, kWaistRoll, kWaistPitch};
+ const int RIGHT0 = 7; // right arm occupies arm[7..13]
+
+ const float kp = 60.f, kd = 1.5f, dt = 0.02f;
+ const float max_delta = 0.5f * dt; // 0.5 rad/s velocity limit
+ const auto sleep_t = std::chrono::milliseconds(20);
+
+ std::array start{};
+ for (int i = 0; i < 17; i++) start[i] = state.motor_state().at(arm[i]).q();
+
+ // Right-arm raise target [ShPitch, ShRoll, ShYaw, Elbow, WrRoll, WrPitch, WrYaw].
+ // Conservative greeting raise (arm out + forearm up). Tune here.
+ const std::array rraise = {0.0f, -1.0f, 0.0f, 1.4f, 0.0f, 0.0f, 0.0f};
+ std::array target = start;
+ for (int j = 0; j < 7; j++) target[RIGHT0 + j] = rraise[j];
+
+ std::array des = start;
+ float weight = 0.f;
+ unitree_hg::msg::dds_::LowCmd_ msg;
+
+ auto publish = [&](float w) {
+ msg.motor_cmd().at(kNotUsedJoint).q(w);
+ for (int i = 0; i < 17; i++) {
+ auto &mc = msg.motor_cmd().at(arm[i]);
+ mc.q(des[i]); mc.dq(0.f); mc.kp(kp); mc.kd(kd); mc.tau(0.f);
+ }
+ pub->Write(msg);
+ };
+
+ auto release = [&]() {
+ std::cout << "Releasing arm (weight -> 0) ..." << std::endl;
+ while (weight > 0.f) {
+ weight = std::max(0.f, weight - 0.015f);
+ publish(weight);
+ std::this_thread::sleep_for(sleep_t);
+ }
+ publish(0.f);
+ };
+
+ // Move 'des' toward 'goal' (velocity limited). Returns false if interrupted.
+ auto move_to = [&](const std::array &goal, int hold_steps) -> bool {
+ for (int step = 0; step < 600; step++) {
+ if (g_stop) return false;
+ float maxerr = 0.f;
+ for (int i = 0; i < 17; i++) {
+ float d = std::clamp(goal[i] - des[i], -max_delta, max_delta);
+ des[i] += d;
+ maxerr = std::max(maxerr, std::abs(goal[i] - des[i]));
+ }
+ publish(1.f);
+ std::this_thread::sleep_for(sleep_t);
+ if (maxerr < 1e-3f) { // arrived -> hold
+ for (int h = 0; h < hold_steps && !g_stop; h++) {
+ publish(1.f);
+ std::this_thread::sleep_for(sleep_t);
+ }
+ return !g_stop;
+ }
+ }
+ return !g_stop;
+ };
+
+ std::cout << "\n*** G1 RIGHT-ARM RAISE TEST ***\n"
+ << "The right arm WILL move. Ensure clear space + E-stop ready.\n"
+ << "Press ENTER to engage the arm (no motion yet) ... " << std::flush;
+ std::cin.get();
+ if (g_stop) { release(); return 0; }
+
+ // Phase 1: engage — ramp weight to 1 while commanding the measured pose (no motion).
+ while (weight < 1.f && !g_stop) {
+ weight = std::min(1.f, weight + 0.015f);
+ publish(weight);
+ std::this_thread::sleep_for(sleep_t);
+ }
+ if (g_stop) { release(); return 0; }
+
+ std::cout << "Engaged. Press ENTER to RAISE the right arm ... " << std::flush;
+ std::cin.get();
+ if (g_stop || !move_to(target, 100)) { release(); return 0; }
+
+ std::cout << "Raised. Press ENTER to LOWER the arm ... " << std::flush;
+ std::cin.get();
+ move_to(start, 25);
+
+ release();
+ std::cout << "Done." << std::endl;
+ return 0;
+}
diff --git a/example/greet.cpp b/example/greet.cpp
new file mode 100644
index 0000000..6f9704d
--- /dev/null
+++ b/example/greet.cpp
@@ -0,0 +1,28 @@
+/**
+ * @file greet.cpp
+ * @brief Combined entry point for the fused arm+hand greetings (see also the
+ * standalone ./handshake and ./thumbup binaries).
+ *
+ * ./greet [handshake|thumbup] [networkInterface] (default iface: eth0)
+ *
+ * Needs ./inspire_g1 running for the fingers. The ARM physically moves —
+ * clear space + E-stop ready.
+ */
+#include "greet_common.hpp"
+
+int main(int argc, char **argv)
+{
+ const std::string what = argc > 1 ? argv[1] : "handshake";
+ const std::string iface = argc > 2 ? argv[2] : "eth0";
+
+ auto arm = greet_init(iface);
+ Hand hand;
+ usleep(300000);
+
+ if (what == "handshake") runHandshake(*arm, hand);
+ else if (what == "thumbup") runThumbup(*arm, hand);
+ else { std::cout << "Usage: ./greet [handshake|thumbup] [networkInterface]\n"; return 1; }
+
+ std::cout << "done." << std::endl;
+ return 0;
+}
diff --git a/example/greet_common.hpp b/example/greet_common.hpp
new file mode 100644
index 0000000..a4f690e
--- /dev/null
+++ b/example/greet_common.hpp
@@ -0,0 +1,102 @@
+/**
+ * @file greet_common.hpp
+ * @brief Shared arm-action + Inspire-hand greeting routines (handshake / thumbup).
+ *
+ * Arm uses Unitree's built-in actions (G1ArmActionClient, balance-aware).
+ * Hand uses the Inspire DDS bridge -> REQUIRES ./inspire_g1 running for fingers.
+ */
+#pragma once
+
+#include "unitree/robot/g1/arm/g1_arm_action_client.hpp"
+#include "unitree/robot/g1/arm/g1_arm_action_error.hpp"
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+using Vec6 = Eigen::Matrix;
+
+// Inspire hand command publisher (open-loop; needs inspire_g1 running).
+class Hand
+{
+public:
+ Hand()
+ {
+ pub_ = std::make_shared>("rt/inspire/cmd");
+ pub_->InitChannel();
+ cmd_.cmds().resize(12);
+ }
+ void set(const Vec6 &right, const Vec6 &left)
+ {
+ for (size_t i = 0; i < 6; i++) { cmd_.cmds()[i].q() = right(i); cmd_.cmds()[i + 6].q() = left(i); }
+ pub_->Write(cmd_);
+ }
+private:
+ unitree_go::msg::dds_::MotorCmds_ cmd_;
+ unitree::robot::ChannelPublisherPtr pub_;
+};
+
+inline Vec6 gv(float p, float r, float m, float i, float tb, float tr) { Vec6 v; v << p, r, m, i, tb, tr; return v; }
+inline void ghold(int ms) { usleep(ms * 1000); }
+
+// q in [0,1], 0=closed 1=open; order pinky,ring,middle,index,thumb_bend,thumb_rot
+static const Vec6 G_OPEN = gv(1, 1, 1, 1, 1, 1);
+static const Vec6 G_THUMBUP = gv(0, 0, 0, 0, 1, 1);
+static const Vec6 G_SHAKE_READY = gv(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
+static const Vec6 G_SHAKE_GRIP = gv(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);
+
+// Execute a built-in arm action; print a helpful message on failure.
+inline bool arm_do(unitree::robot::g1::G1ArmActionClient &arm, int id, const char *name)
+{
+ using namespace unitree::robot::g1;
+ std::cout << "arm: " << name << " (" << id << ") ..." << std::endl;
+ int32_t ret = arm.ExecuteAction(id);
+ if (ret != 0) {
+ std::cerr << " arm action '" << name << "' failed, code " << ret;
+ if (ret == UT_ROBOT_ARM_ACTION_ERR_ARMSDK) std::cerr << " (rt/arm_sdk occupied)";
+ else if (ret == UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID) std::cerr << " (need fsm {500,501,801})";
+ else if (ret == UT_ROBOT_ARM_ACTION_ERR_HOLDING) std::cerr << " (arm holding; send release 99)";
+ std::cerr << std::endl;
+ return false;
+ }
+ return true;
+}
+
+// arm reaches out -> hand grips -> thumbs-up -> arm releases
+inline void runHandshake(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
+{
+ hand.set(G_SHAKE_READY, G_OPEN); // present an open hand
+ if (!arm_do(arm, 27, "shake hand")) { hand.set(G_OPEN, G_OPEN); return; }
+ ghold(300);
+ std::cout << "hand: grip" << std::endl;
+ hand.set(G_SHAKE_GRIP, G_OPEN); ghold(1500);
+ std::cout << "hand: thumbs-up" << std::endl;
+ hand.set(G_THUMBUP, G_OPEN); ghold(1200);
+ hand.set(G_OPEN, G_OPEN); ghold(400);
+ arm_do(arm, 99, "release arm");
+}
+
+// raise right hand -> hand thumbs-up -> arm releases
+inline void runThumbup(unitree::robot::g1::G1ArmActionClient &arm, Hand &hand)
+{
+ if (!arm_do(arm, 23, "right hand up")) { hand.set(G_OPEN, G_OPEN); return; }
+ ghold(300);
+ std::cout << "hand: thumbs-up" << std::endl;
+ hand.set(G_THUMBUP, G_OPEN); ghold(2500);
+ hand.set(G_OPEN, G_OPEN); ghold(400);
+ arm_do(arm, 99, "release arm");
+}
+
+// Shared setup: init DDS + arm client + hand, then return the arm client.
+inline std::shared_ptr greet_init(const std::string &iface)
+{
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+ auto arm = std::make_shared();
+ arm->Init();
+ arm->SetTimeout(10.f);
+ return arm;
+}
diff --git a/example/hand_bridge.cpp b/example/hand_bridge.cpp
new file mode 100644
index 0000000..447b49a
--- /dev/null
+++ b/example/hand_bridge.cpp
@@ -0,0 +1,208 @@
+/**
+ * @file hand_bridge.cpp
+ * @brief Local TCP <-> DDS bridge for the web dashboard.
+ * - "v0 v1 ... v11\n" -> publish MotorCmds_ to rt/inspire/cmd (q in [0,1], or <0 = release)
+ * - "R\n" -> reply with the 12 latest rt/inspire/state q values (read-back / capture)
+ * - "S\n" -> reply with 24 values: 12 angles then 12 forces (joint tracker)
+ * Persistent publisher + state subscriber. Needs ./inspire_g1 running.
+ *
+ * ./hand_bridge [iface] [port] (default eth0 7799)
+ */
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+int main(int argc, char **argv)
+{
+ std::string iface = argc > 1 ? argv[1] : "eth0";
+ int port = argc > 2 ? std::atoi(argv[2]) : 7799;
+
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+ auto pub = std::make_shared>("rt/inspire/cmd");
+ pub->InitChannel();
+ unitree_go::msg::dds_::MotorCmds_ cmd;
+ cmd.cmds().resize(12);
+
+ // Subscribe to hand state for read-back / capture.
+ std::mutex mtx;
+ double state[12] = {0};
+ double force[12] = {0};
+ double cur[12] = {0}, fset[12] = {0}; // dq = current mA, ddq = force limit
+ double temp[12] = {0}, err[12] = {0}, sta[12] = {0}, lost[12] = {0};
+ double rate = 0; int rn = 0; auto rt0 = std::chrono::steady_clock::now();
+
+ // ---- force-follow (admittance) --------------------------------------------------
+ // The RH56 cannot be back-driven, so pushing a finger moves it 0.000. This closes the
+ // loop in software: read the press, drive the finger that way, hold when you stop. It
+ // lives here rather than in the web layer because the loop needs the 30Hz state stream
+ // and the command publisher in the same place -- polling over HTTP is far too slow.
+ std::atomic follow{false};
+ std::atomic fgain{0.0006}, fdead{60.0}, fmaxrate{0.35}, fforce{400.0};
+ double ftarget[12] = {0}, fbase[12] = {0};
+ std::atomic fcal{false}; // tau_est, in grams — the joint tracker needs BOTH angle and
+ // force, because 'force moved but angle did not' is the whole
+ // signature of a finger that cannot be back-driven.
+ auto sub = std::make_shared>("rt/inspire/state");
+ sub->InitChannel([&](const void *m) {
+ auto s = (const unitree_go::msg::dds_::MotorStates_ *)m;
+ std::lock_guard lk(mtx);
+ for (int i = 0; i < 12 && i < (int)s->states().size(); i++) {
+ state[i] = s->states()[i].q();
+ force[i] = s->states()[i].tau_est();
+ cur[i] = s->states()[i].dq(); // mA
+ fset[i] = s->states()[i].ddq(); // grip force limit
+ temp[i] = s->states()[i].temperature();
+ err[i] = s->states()[i].reserve()[0];
+ sta[i] = s->states()[i].reserve()[1];
+ lost[i] = s->states()[i].lost();
+ }
+ // Measure the ACTUAL publish rate here — a hand whose bus has died keeps serving
+ // cached values, so a healthy-looking readout with a collapsed rate is the tell.
+ if (++rn >= 20) {
+ auto now = std::chrono::steady_clock::now();
+ double dt = std::chrono::duration(now - rt0).count();
+ if (dt > 0) rate = rn / dt;
+ rn = 0; rt0 = now;
+ }
+ });
+
+ // Admittance thread: only publishes while follow is on, so it never fights the
+ // dashboard, the recorder, or anything else driving the hand.
+ std::thread ftick([&]{
+ auto prev = std::chrono::steady_clock::now();
+ while (true) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(33));
+ auto now = std::chrono::steady_clock::now();
+ double dt = std::chrono::duration(now - prev).count(); prev = now;
+ if (!follow.load()) continue;
+ if (dt > 0.2) dt = 0.2;
+ {
+ std::lock_guard lk(mtx);
+ if (fcal.exchange(false)) { // arm: zero on the current force
+ for (int i = 0; i < 12; i++) { fbase[i] = force[i]; ftarget[i] = state[i]; }
+ }
+ for (int i = 0; i < 12; i++) {
+ double dev = force[i] - fbase[i];
+ if (std::abs(dev) <= fdead.load()) continue;
+ dev -= (dev > 0 ? fdead.load() : -fdead.load());
+ double step = -fgain.load() * dev * dt; // press pad (+force) -> close
+ double cap = fmaxrate.load() * dt;
+ if (step > cap) step = cap;
+ if (step < -cap) step = -cap;
+ ftarget[i] += step;
+ if (ftarget[i] < 0.0) ftarget[i] = 0.0;
+ if (ftarget[i] > 1.0) ftarget[i] = 1.0;
+ }
+ for (int i = 0; i < 12; i++) {
+ cmd.cmds()[i].q() = ftarget[i];
+ cmd.cmds()[i].kp() = fforce.load();
+ }
+ }
+ pub->Write(cmd);
+ }
+ });
+ ftick.detach();
+
+ int srv = socket(AF_INET, SOCK_STREAM, 0);
+ int opt = 1;
+ setsockopt(srv, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
+ sockaddr_in addr{};
+ addr.sin_family = AF_INET;
+ addr.sin_addr.s_addr = inet_addr("127.0.0.1");
+ addr.sin_port = htons(port);
+ if (bind(srv, (sockaddr *)&addr, sizeof(addr)) < 0) { perror("bind"); return 1; }
+ listen(srv, 8);
+ std::cout << "hand_bridge listening on 127.0.0.1:" << port << " (iface " << iface << ")" << std::endl;
+
+ char buf[1024];
+ while (true)
+ {
+ int c = accept(srv, nullptr, nullptr);
+ if (c < 0) continue;
+ int n = read(c, buf, sizeof(buf) - 1);
+ if (n > 0)
+ {
+ buf[n] = 0;
+ if (buf[0] == 'F' || buf[0] == 'f') // follow: "F 1 gain dead maxrate force" | "F 0"
+ {
+ std::istringstream iss(buf + 1);
+ int on = 0; double g = 0, d = 0, mr = 0, fo = 0;
+ iss >> on;
+ if (iss >> g && g > 0) fgain.store(g);
+ if (iss >> d && d > 0) fdead.store(d);
+ if (iss >> mr && mr > 0) fmaxrate.store(mr);
+ if (iss >> fo && fo > 0) fforce.store(fo);
+ if (on) fcal.store(true); // re-zero the baseline each time it arms
+ follow.store(on != 0);
+ std::string o = std::string("follow ") + (on ? "on" : "off") + "\n";
+ (void)!write(c, o.c_str(), o.size());
+ }
+ else if (buf[0] == 'D' || buf[0] == 'd') // full diagnostics: 8x12 + rate
+ {
+ std::string out;
+ { std::lock_guard lk(mtx);
+ char t[32];
+ const double *blocks[8] = {state, force, cur, fset, temp, err, sta, lost};
+ for (int b = 0; b < 8; b++)
+ for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", blocks[b][i]); out += t; }
+ snprintf(t, sizeof(t), "%.2f ", rate); out += t; }
+ out += "\n";
+ (void)!write(c, out.c_str(), out.size());
+ }
+ else if (buf[0] == 'S' || buf[0] == 's') // angle + force: 24 values
+ {
+ std::string out;
+ { std::lock_guard lk(mtx);
+ char t[32];
+ for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; }
+ for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.1f ", force[i]); out += t; } }
+ out += "\n";
+ (void)!write(c, out.c_str(), out.size());
+ }
+ else if (buf[0] == 'R' || buf[0] == 'r') // read-back request (12 angles, legacy)
+ {
+ std::string out;
+ { std::lock_guard lk(mtx);
+ char t[32];
+ for (int i = 0; i < 12; i++) { snprintf(t, sizeof(t), "%.3f ", state[i]); out += t; } }
+ out += "\n";
+ (void)!write(c, out.c_str(), out.size());
+ }
+ else
+ {
+ std::istringstream iss(buf);
+ std::vector v; float x;
+ while (iss >> x) v.push_back(x);
+ if (v.size() == 12)
+ {
+ for (int i = 0; i < 12; i++) cmd.cmds()[i].q() = v[i];
+ for (int k = 0; k < 3; k++) { pub->Write(cmd); usleep(8000); }
+ (void)!write(c, "ok\n", 3);
+ }
+ else (void)!write(c, "err\n", 4);
+ }
+ }
+ close(c);
+ }
+ return 0;
+}
diff --git a/example/hand_diag.cpp b/example/hand_diag.cpp
new file mode 100644
index 0000000..fd3c8a4
--- /dev/null
+++ b/example/hand_diag.cpp
@@ -0,0 +1,290 @@
+/**
+ * @file hand_diag.cpp
+ * @brief Read the Inspire RH56 fault registers per finger: ERROR, STATUS, TEMP,
+ * CURRENT. Tells you exactly WHY a finger won't move (locked rotor,
+ * over-temperature, over-current, ...) instead of guessing.
+ *
+ * Register map (from the Inspire Modbus map; this serial protocol encodes the
+ * address as low,high in bytes 5,6 — verified: CLEAR_ERROR 1004 = 0x03EC = {0xEC,0x03}):
+ * CURRENT 1594 (0x063A, 12 bytes) ERROR 1606 (0x0646, 6)
+ * STATUS 1612 (0x064C, 6) TEMP 1618 (0x0652, 6)
+ * CLEAR_ERROR 1004 (0x03EC) RESET_PARA 1006 (0x03EE)
+ *
+ * g++ -std=c++17 -Iinclude example/hand_diag.cpp -o /tmp/hand_diag
+ * /tmp/hand_diag /dev/ttyUSB1 [clear] # 'clear' also sends CLEAR_ERROR
+ *
+ * Needs the port free (stop the container first).
+ */
+#include "SerialPort.h"
+#include
+#include
+#include
+#include
+#include
+#include
+
+static const char *FING[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
+
+static uint8_t cksum(const uint8_t *d, int len)
+{
+ uint8_t s = 0;
+ for (int i = 2; i < len - 1; i++) s += d[i];
+ return s;
+}
+
+// Read `n` bytes from register `addr`, robustly (flush + re-query).
+static bool readReg(SerialPort &sp, uint16_t addr, uint8_t n, uint8_t *out, uint8_t id = 1)
+{
+ uint8_t q[9] = {0xEB, 0x90, id, 0x04, 0x11,
+ (uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8), n, 0x00};
+ q[8] = cksum(q, 9);
+ uint8_t buf[128];
+ for (int att = 0; att < 8; att++)
+ {
+ sp.flush();
+ sp.send(q, 9);
+ usleep(8000);
+ size_t got = sp.recv(buf, sizeof(buf));
+ for (size_t off = 0; off + 7 + n <= got; off++)
+ {
+ if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
+ (buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
+ if (buf[off + 4] != 0x11) continue;
+ uint16_t got_addr = buf[off + 5] | (buf[off + 6] << 8);
+ if (got_addr != addr) continue;
+ int frame = 7 + n + 1; // header+addr+data+checksum
+ if (off + frame > got) continue;
+ if (buf[off + frame - 1] != cksum(buf + off, frame)) continue;
+ memcpy(out, buf + off + 7, n);
+ return true;
+ }
+ }
+ return false;
+}
+
+static void writeReg16(SerialPort &sp, uint16_t addr, uint16_t val, uint8_t id = 1)
+{
+ uint8_t c[11] = {0xEB, 0x90, id, 0x06, 0x12,
+ (uint8_t)(addr & 0xFF), (uint8_t)(addr >> 8),
+ (uint8_t)(val & 0xFF), (uint8_t)(val >> 8), 0x00, 0x00};
+ c[9] = cksum(c, 10);
+ sp.send(c, 10);
+ usleep(5000);
+ uint8_t b[32];
+ sp.recv(b, sizeof(b));
+}
+
+static std::string errStr(uint8_t e)
+{
+ if (!e) return "OK";
+ std::string s;
+ if (e & 0x01) s += "LOCKED_ROTOR ";
+ if (e & 0x02) s += "OVER_TEMPERATURE ";
+ if (e & 0x04) s += "OVER_CURRENT ";
+ if (e & 0x08) s += "ABNORMAL_OPERATION ";
+ if (e & 0x10) s += "COMMUNICATION_ERROR ";
+ return s;
+}
+
+// Command all 6 fingers to a position (ANGLE_SET 1486 = 0x05CE, 12 bytes).
+static void setAll(SerialPort &sp, int16_t val, uint8_t id = 1)
+{
+ uint8_t c[20];
+ c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
+ c[5] = 0xCE; c[6] = 0x05;
+ for (int i = 0; i < 6; i++)
+ {
+ c[7 + 2 * i] = val & 0xFF;
+ c[8 + 2 * i] = (val >> 8) & 0xFF;
+ }
+ c[19] = cksum(c, 20);
+ sp.send(c, 20);
+ usleep(5000);
+ uint8_t b[32];
+ sp.recv(b, sizeof(b));
+}
+
+// Write all 6 force limits (FORCE_SET 1498 = 0x05DA, 12 bytes).
+static void setForceAll(SerialPort &sp, uint16_t val, uint8_t id = 1)
+{
+ uint8_t c[20];
+ c[0] = 0xEB; c[1] = 0x90; c[2] = id; c[3] = 0x0F; c[4] = 0x12;
+ c[5] = 0xDA; c[6] = 0x05;
+ for (int i = 0; i < 6; i++)
+ {
+ c[7 + 2 * i] = val & 0xFF;
+ c[8 + 2 * i] = (val >> 8) & 0xFF;
+ }
+ c[19] = cksum(c, 20);
+ sp.send(c, 20);
+ usleep(5000);
+ uint8_t b[32];
+ sp.recv(b, sizeof(b));
+}
+
+// Does the motor actually energize when told to move? current>0 => it's trying
+// (mechanically blocked); current==0 => it never energizes.
+//
+// IMPORTANT: current==0 alone does NOT mean the actuator is dead. The RH56 stops driving a
+// finger the instant its MEASURED force (FORCE_ACT 1582) reaches its force LIMIT
+// (FORCE_SET 1498), so a finger whose force-sensor zero has drifted above the limit draws
+// 0 mA and is indistinguishable from a dead motor. This test used to inherit whatever limit
+// the service last wrote (500g) and so reproduced exactly that failure while looking like an
+// independent raw-serial check — it is how the G1's right mid+index were wrongly called dead
+// when they in fact run their full range at a 1000g limit. It now sweeps the limit itself.
+static void moveTest(SerialPort &sp, uint16_t force)
+{
+ printf("\n=== MOVE TEST: force limit %ug, command CLOSE, watch current + force + angle ===\n",
+ force);
+ setForceAll(sp, force);
+ setAll(sp, 1000); // open first
+ usleep(800000);
+
+ uint8_t f0[12] = {0};
+ if (readReg(sp, 1582, 12, f0))
+ {
+ printf("resting force vs the %ug limit (>= limit => the driver refuses to move it):\n ",
+ force);
+ for (int i = 0; i < 6; i++)
+ {
+ int16_t F = (int16_t)(f0[2 * i] | (f0[2 * i + 1] << 8));
+ printf("%s=%dg%s ", FING[i], F, (F >= (int)force || -F >= (int)force) ? "<", "hand_diag ");
+}
+
+// Are the dead channels merely CONFIGURED not to drive? A speed / force / current
+// limit of 0 would give exactly the observed signature: no motion, no current, no
+// error, status "reached target". That would be software-fixable.
+static void paramDump(SerialPort &sp)
+{
+ struct { const char *name; uint16_t addr; uint8_t bytes; } regs[] = {
+ {"ANGLE_SET ", 1486, 12},
+ {"FORCE_SET ", 1498, 12},
+ {"SPEED_SET ", 1522, 12},
+ {"CURRENT_LIM ", 1020, 12},
+ {"ANGLE_ACT ", 1546, 12},
+ };
+ printf("%-13s", "register");
+ for (int i = 0; i < 6; i++) printf("%9s", FING[i]);
+ printf("\n");
+ for (auto &r : regs)
+ {
+ uint8_t b[16] = {0};
+ bool ok = readReg(sp, r.addr, r.bytes, b);
+ printf("%-13s", r.name);
+ for (int i = 0; i < 6; i++)
+ {
+ if (!ok) { printf("%9s", "FAIL"); continue; }
+ printf("%9d", (int16_t)(b[2 * i] | (b[2 * i + 1] << 8)));
+ }
+ printf("\n");
+ }
+ printf("\n(a 0 in SPEED_SET / FORCE_SET / CURRENT_LIM for a finger would explain\n"
+ " no motion + no current + no error, and would be software-fixable.)\n");
+}
+
+int main(int argc, char **argv)
+{
+ const char *dev = argc > 1 ? argv[1] : "/dev/ttyUSB1";
+ std::string opt = argc > 2 ? argv[2] : "";
+ bool doClear = (opt == "clear");
+ bool doMove = (opt == "move");
+ SerialPort sp(dev, B115200, 8);
+ if (opt == "params") { paramDump(sp); return 0; }
+ // Command a CLOSE, then read ANGLE_SET back: does the setpoint actually LAND in
+ // the dead channels? ANGLE_SET=0 but no motion => setpoint accepted, motor dead.
+ // ANGLE_SET stays 1000 => the channel rejects the write.
+ if (opt == "setclose")
+ {
+ printf("commanding CLOSE (ANGLE_SET=0) ...\n\n");
+ setAll(sp, 0);
+ usleep(1500000);
+ paramDump(sp);
+ setAll(sp, 1000);
+ return 0;
+ }
+ // 'move [force]' — force defaults to the RH56 max so a drifted sensor zero can't
+ // silently gate a healthy finger off and make it look dead.
+ if (doMove)
+ {
+ uint16_t f = argc > 3 ? (uint16_t)atoi(argv[3]) : 1000;
+ moveTest(sp, f);
+ return 0;
+ }
+
+ if (doClear)
+ {
+ printf("sending CLEAR_ERROR (1004) ...\n");
+ writeReg16(sp, 1004, 1);
+ usleep(300000);
+ }
+
+ uint8_t err[6] = {0}, sta[6] = {0}, tmp[6] = {0}, cur[12] = {0};
+ uint8_t fact[12] = {0}, fset[12] = {0};
+ bool eok = readReg(sp, 1606, 6, err);
+ bool sok = readReg(sp, 1612, 6, sta);
+ bool tok = readReg(sp, 1618, 6, tmp);
+ bool cok = readReg(sp, 1594, 12, cur);
+ bool faok = readReg(sp, 1582, 12, fact); // FORCE_ACT — measured
+ bool fsok = readReg(sp, 1498, 12, fset); // FORCE_SET — the limit
+
+ printf("port %s (error=%s status=%s temp=%s current=%s force=%s limit=%s)\n\n", dev,
+ eok ? "ok" : "FAIL", sok ? "ok" : "FAIL", tok ? "ok" : "FAIL", cok ? "ok" : "FAIL",
+ faok ? "ok" : "FAIL", fsok ? "ok" : "FAIL");
+ printf("%-8s %6s %8s %6s %9s %8s %7s %s\n",
+ "finger", "err", "status", "temp", "current", "force", "limit", "meaning");
+ bool anyGated = false;
+ for (int i = 0; i < 6; i++)
+ {
+ int16_t c = cok ? (int16_t)(cur[2 * i] | (cur[2 * i + 1] << 8)) : 0;
+ int16_t fa = faok ? (int16_t)(fact[2 * i] | (fact[2 * i + 1] << 8)) : 0;
+ int16_t fs = fsok ? (int16_t)(fset[2 * i] | (fset[2 * i + 1] << 8)) : 0;
+ // THE check that was missing: a finger already at/over its force limit is gated off
+ // by the driver and will read 0 current with no error — identical to a dead motor.
+ bool gated = faok && fsok && fs > 0 && (fa >= fs || -fa >= fs);
+ if (gated) anyGated = true;
+ std::string mean = eok ? errStr(err[i]) : "?";
+ if (gated) mean = "FORCE-GATED (not dead: force >= limit)";
+ printf("%-8s 0x%02X %8d %5d\xC2\xB0 %8d %7dg %6dg %s\n",
+ FING[i], eok ? err[i] : 0, sok ? sta[i] : -1, tok ? tmp[i] : -1, c,
+ fa, fs, mean.c_str());
+ }
+ if (anyGated)
+ printf("\n>> One or more fingers is FORCE-GATED: its measured force already meets its\n"
+ ">> limit, so the driver refuses to energise it (0 current, no error). This is a\n"
+ ">> drifted force-sensor zero, NOT a dead actuator. Confirm with:\n"
+ ">> %s %s move 1000\n"
+ ">> Fix by re-zeroing the sensor with the hand unloaded, or raise the limit.\n",
+ argv[0], dev);
+ return 0;
+}
diff --git a/example/hand_forcetest.cpp b/example/hand_forcetest.cpp
new file mode 100644
index 0000000..048e664
--- /dev/null
+++ b/example/hand_forcetest.cpp
@@ -0,0 +1,135 @@
+/**
+ * @file hand_forcetest.cpp
+ * @brief Read BOTH the actual angle (0x0A) and the force sensor (0x2E) of the
+ * right hand while it holds open, to see whether pushing a finger by hand
+ * registers as a FORCE change even though the position can't move
+ * (non-backdrivable). Robust re-query read (flush + resend + scan).
+ *
+ * g++ -std=c++17 -Iinclude example/hand_forcetest.cpp -o /tmp/hft
+ * /tmp/hft /dev/ttyUSB1 (right hand; needs the port free)
+ */
+#include "inspire.h"
+#include
+#include
+#include
+#include
+#include
+
+// Robust read of a 6-value register (angleAct 0x0A or forceAct 0x2E).
+static int robustRead(SerialPort &sp, uint8_t addr, uint8_t id,
+ Eigen::Matrix &out, double scale, bool sign)
+{
+ std::vector q = {0xEB, 0x90, id, 0x04, 0x11, addr, 0x06, 0x0C, 0x00};
+ uint8_t s = 0;
+ for (size_t i = 2; i < q.size() - 1; i++) s += q[i];
+ q.back() = s;
+
+ uint8_t buf[64];
+ for (int att = 0; att < 8; att++)
+ {
+ sp.flush();
+ sp.send(q.data(), q.size());
+ usleep(8000);
+ size_t n = sp.recv(buf, sizeof(buf));
+ for (size_t off = 0; off + 20 <= n; off++)
+ {
+ if (!((buf[off] == 0x90 && buf[off + 1] == 0xEB) ||
+ (buf[off] == 0xEB && buf[off + 1] == 0x90))) continue;
+ if (buf[off + 4] != 0x11 || buf[off + 5] != addr) continue;
+ uint8_t cs = 0;
+ for (int i = 2; i < 19; i++) cs += buf[off + i];
+ if (buf[off + 19] != cs) continue;
+ const uint8_t *b = buf + off;
+ for (int i = 0; i < 6; i++)
+ {
+ uint16_t raw = b[7 + 2 * i] | (b[8 + 2 * i] << 8);
+ out(i) = (sign ? (double)(int16_t)raw : (double)raw) * scale;
+ }
+ return 0;
+ }
+ }
+ return 1;
+}
+
+int main(int argc, char **argv)
+{
+ const char *pr = argc > 1 ? argv[1] : "/dev/ttyUSB1";
+ auto sp = std::make_shared(pr, B115200, 6);
+ inspire::InspireHand right(sp, 1);
+ right.ClearError();
+ right.SetVelocity(1000, 1000, 1000, 1000, 1000, 1000);
+ right.SetForce(500, 500, 500, 500, 500, 500);
+
+ Eigen::Matrix qopen, pos, force;
+ for (int i = 0; i < 6; i++) qopen(i) = 1.0;
+
+ // DIAGNOSTIC: mimic inspire_g1's tight loop (SetPos, settle, GetPos, GetForce,
+ // no inter-cycle sleep) and count how often each read fails + whether force
+ // actually changes. This reproduces the "force frozen" symptom.
+ if (argc > 2 && std::string(argv[2]) == "diag")
+ {
+ int posFail = 0, forceFail = 0, N = 200;
+ double fmin[6], fmax[6];
+ for (int i = 0; i < 6; i++) { fmin[i] = 1e9; fmax[i] = -1e9; }
+ for (int k = 0; k < N; k++)
+ {
+ right.SetPosition(qopen);
+ usleep(3000);
+ if (robustRead(*sp, 0x0A, 1, pos, 1.0 / 1000.0, false) != 0) posFail++;
+ if (robustRead(*sp, 0x2E, 1, force, 1.0, true) != 0) forceFail++;
+ else for (int i = 0; i < 6; i++) { if (force(i) < fmin[i]) fmin[i] = force(i); if (force(i) > fmax[i]) fmax[i] = force(i); }
+ }
+ printf("tight loop N=%d: GetPosition fails=%d GetForce fails=%d\n", N, posFail, forceFail);
+ printf("force range over run (g): ");
+ for (int i = 0; i < 6; i++) printf("[%.0f..%.0f] ", fmin[i], fmax[i]);
+ printf("\n");
+ return 0;
+ }
+
+ const char *NM[6] = {"pinky", "ring", "mid", "index", "thumbB", "thumbR"};
+ const double THRESH = 100.0; // grams of deviation from baseline = a deliberate push
+
+ // Drive open + hold, and calibrate each finger's resting-force baseline (~2s).
+ printf("Calibrating resting baseline — DON'T touch the fingers for 2s...\n");
+ double base[6] = {0, 0, 0, 0, 0, 0};
+ int nb = 0;
+ for (int k = 0; k < 10; k++)
+ {
+ right.SetPosition(qopen);
+ usleep(3000);
+ if (robustRead(*sp, 0x2E, 1, force, 1.0, true) == 0)
+ {
+ for (int i = 0; i < 6; i++) base[i] += force(i);
+ nb++;
+ }
+ usleep(150000);
+ }
+ for (int i = 0; i < 6; i++) base[i] = nb ? base[i] / nb : 0;
+ printf("Baseline(g): ");
+ for (int i = 0; i < 6; i++) printf("%s=%.0f ", NM[i], base[i]);
+ printf("\n\nNow PUSH one right finger at a time — it flags which finger and how hard:\n\n");
+
+ for (int k = 0; k < 120; k++)
+ {
+ right.SetPosition(qopen);
+ usleep(3000);
+ int rf = robustRead(*sp, 0x2E, 1, force, 1.0, true);
+ if (rf == 0)
+ {
+ char line[256]; int p = 0; bool pushed = false;
+ for (int i = 0; i < 6; i++)
+ {
+ double dev = force(i) - base[i];
+ if (dev > THRESH || dev < -THRESH)
+ {
+ pushed = true;
+ p += sprintf(line + p, "%s:%+.0fg ", NM[i], dev);
+ }
+ }
+ if (pushed) printf("k%3d PUSH -> %s\n", k, line);
+ }
+ usleep(120000);
+ }
+ printf("(done)\n");
+ return 0;
+}
diff --git a/example/hand_gestures.cpp b/example/hand_gestures.cpp
new file mode 100644
index 0000000..3004845
--- /dev/null
+++ b/example/hand_gestures.cpp
@@ -0,0 +1,188 @@
+/**
+ * @file hand_gestures.cpp
+ * @brief Named gestures for the Inspire RH56 hands on G1, over DDS (rt/inspire/cmd).
+ *
+ * Requires the service running in another terminal: ./inspire_g1
+ *
+ * Usage:
+ * ./hand_gestures # interactive menu (pick gestures repeatedly)
+ * ./hand_gestures [iface] # run one gesture and exit
+ * gesture = open | close | box | thumbup | handshake | combo | demo
+ *
+ * Per-hand DOF order: [pinky, ring, middle, index, thumb_bend, thumb_rotation]
+ * q in [0,1]: 0 = closed, 1 = open. Indices 0-5 = right hand, 6-11 = left hand.
+ *
+ * NOTE: this controls the HAND (fingers) only. Raising the arm is a separate
+ * subsystem (G1 arm joints) and is not driven here.
+ */
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+using Vec6 = Eigen::Matrix;
+
+class HandController
+{
+public:
+ HandController() { Init(); }
+
+ void set(const Vec6 &right, const Vec6 &left)
+ {
+ for (size_t i = 0; i < 6; i++)
+ {
+ cmd.cmds()[i].q() = right(i);
+ cmd.cmds()[i + 6].q() = left(i);
+ }
+ handcmd->Write(cmd);
+ }
+ void setBoth(const Vec6 &q) { set(q, q); }
+
+ unitree_go::msg::dds_::MotorCmds_ cmd;
+ unitree_go::msg::dds_::MotorStates_ state;
+
+private:
+ void Init()
+ {
+ handcmd = std::make_shared>("rt/inspire/cmd");
+ handcmd->InitChannel();
+ cmd.cmds().resize(12);
+ handstate = std::make_shared>("rt/inspire/state");
+ handstate->InitChannel([this](const void *message) {
+ std::lock_guard lock(mtx);
+ state = *(unitree_go::msg::dds_::MotorStates_ *)message;
+ });
+ state.states().resize(12);
+ }
+
+ std::mutex mtx;
+ unitree::robot::ChannelPublisherPtr handcmd;
+ unitree::robot::ChannelSubscriberPtr handstate;
+};
+
+static Vec6 V(float pinky, float ring, float middle, float index, float thumb_bend, float thumb_rot)
+{
+ Vec6 v; v << pinky, ring, middle, index, thumb_bend, thumb_rot; return v;
+}
+static void hold(int ms) { usleep(ms * 1000); }
+
+// --- Pose library (tune freely; values are [0,1], 0=closed 1=open) ---
+static const Vec6 OPEN = V(1, 1, 1, 1, 1, 1);
+static const Vec6 CLOSE = V(0, 0, 0, 0, 0, 0);
+static const Vec6 BOX = V(0.5, 0.5, 0.5, 0.5, 0.4, 0.1);
+static const Vec6 THUMBUP = V(0, 0, 0, 0, 1, 1);
+static const Vec6 SHAKE_READY = V(0.85, 0.85, 0.85, 0.85, 0.85, 0.6);
+static const Vec6 SHAKE_GRIP = V(0.2, 0.2, 0.2, 0.2, 0.25, 0.1);
+
+// --- Gestures (social ones use the RIGHT hand, left held open) ---
+static void g_pose(HandController &h, const Vec6 &q, const char *name, int ms = 1500)
+{
+ std::cout << " [" << name << "]" << std::endl;
+ h.setBoth(q);
+ hold(ms);
+}
+static void g_thumbup(HandController &h, int ms = 2000)
+{
+ std::cout << " [thumbup] (right hand)" << std::endl;
+ h.set(THUMBUP, OPEN);
+ hold(ms);
+}
+static void g_handshake(HandController &h)
+{
+ std::cout << " [handshake] (right hand)" << std::endl;
+ h.set(SHAKE_READY, OPEN); hold(1200); // reach out, open
+ h.set(SHAKE_GRIP, OPEN); hold(800); // grip
+ for (int k = 0; k < 3; k++) // a few firm "shakes"
+ {
+ h.set(SHAKE_GRIP * 0.85f, OPEN); hold(180);
+ h.set(SHAKE_GRIP, OPEN); hold(180);
+ }
+ h.set(THUMBUP, OPEN); hold(1500); // finish with a thumbs-up
+ h.set(OPEN, OPEN); hold(600); // relax
+}
+static void g_demo(HandController &h)
+{
+ g_pose(h, OPEN, "open");
+ g_pose(h, CLOSE, "close");
+ g_pose(h, BOX, "box");
+ g_pose(h, OPEN, "open");
+ g_thumbup(h);
+ g_handshake(h);
+ g_pose(h, OPEN, "open");
+}
+
+// Dispatch by name or menu number. Returns false only on quit.
+static bool runGesture(HandController &h, const std::string &g)
+{
+ if (g == "open" || g == "1") g_pose(h, OPEN, "open");
+ else if (g == "close" || g == "2") g_pose(h, CLOSE, "close");
+ else if (g == "box" || g == "3") g_pose(h, BOX, "box");
+ else if (g == "thumbup" || g == "4") g_thumbup(h);
+ else if (g == "handshake" || g == "5") g_handshake(h);
+ else if (g == "combo" || g == "6") { g_thumbup(h); g_handshake(h); }
+ else if (g == "demo" || g == "7") g_demo(h);
+ else if (g == "q" || g == "quit" || g == "0") return false;
+ else std::cout << " unknown option: '" << g << "'" << std::endl;
+ return true;
+}
+
+static void printMenu()
+{
+ std::cout << "\n=== Inspire Hand Gestures ===\n"
+ << " 1) open\n"
+ << " 2) close (fist)\n"
+ << " 3) box (power grasp)\n"
+ << " 4) thumbup (right hand)\n"
+ << " 5) handshake (right hand, ends thumbs-up)\n"
+ << " 6) combo (thumbup + handshake)\n"
+ << " 7) demo (run all)\n"
+ << " q) quit\n"
+ << "Select: " << std::flush;
+}
+
+static std::string trim(std::string s)
+{
+ const char *ws = " \t\r\n";
+ size_t a = s.find_first_not_of(ws);
+ size_t b = s.find_last_not_of(ws);
+ return a == std::string::npos ? "" : s.substr(a, b - a + 1);
+}
+
+int main(int argc, char **argv)
+{
+ std::string arg1 = argc > 1 ? argv[1] : "";
+ std::string iface = argc > 2 ? argv[2] : "";
+
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+ auto hand = std::make_shared();
+ usleep(300000); // let DDS match the service
+
+ // Direct mode: run one gesture and exit (backward compatible).
+ if (!arg1.empty())
+ {
+ runGesture(*hand, arg1);
+ std::cout << "done." << std::endl;
+ return 0;
+ }
+
+ // Interactive menu: fire gestures repeatedly over one DDS connection.
+ std::string line;
+ while (true)
+ {
+ printMenu();
+ if (!std::getline(std::cin, line)) break; // EOF / Ctrl-D
+ line = trim(line);
+ if (line.empty()) continue;
+ if (!runGesture(*hand, line)) break;
+ }
+ std::cout << "bye." << std::endl;
+ return 0;
+}
diff --git a/example/hand_pose.cpp b/example/hand_pose.cpp
new file mode 100644
index 0000000..64c9981
--- /dev/null
+++ b/example/hand_pose.cpp
@@ -0,0 +1,61 @@
+/**
+ * @file hand_pose.cpp
+ * @brief Live finger-pose tuner for the Inspire hand (right hand). Needs ./inspire_g1.
+ *
+ * ./hand_pose [networkInterface] (default iface: eth0)
+ *
+ * Then type 6 numbers (q in [0,1], 0=closed 1=open):
+ * pinky ring middle index thumb_bend thumb_rot
+ * e.g. 0 0 0 0 1 1 -> try a thumbs-up and tweak the last two values
+ * Shortcuts: open | close | q(uit)
+ * The pose is applied to the RIGHT hand (left held open) and holds until the next entry.
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+int main(int argc, char **argv)
+{
+ const std::string iface = argc > 1 ? argv[1] : "eth0";
+ unitree::robot::ChannelFactory::Instance()->Init(0, iface);
+
+ auto pub = std::make_shared>("rt/inspire/cmd");
+ pub->InitChannel();
+ unitree_go::msg::dds_::MotorCmds_ cmd;
+ cmd.cmds().resize(12);
+ usleep(300000);
+
+ // right = indices 0-5, left = 6-11 (held open)
+ auto apply = [&](const std::vector &r) {
+ for (int i = 0; i < 6; i++) { cmd.cmds()[i].q() = r[i]; cmd.cmds()[i + 6].q() = 1.0f; }
+ for (int n = 0; n < 12; n++) { pub->Write(cmd); usleep(30000); } // ~0.36s of repeats so it lands + moves
+ };
+
+ std::cout << "Inspire RIGHT-hand pose tuner. Order: pinky ring middle index thumb_bend thumb_rot\n"
+ << "q in [0,1], 0=closed 1=open. Type 6 numbers, or 'open'/'close'/'q'.\n";
+
+ std::string line;
+ while (true) {
+ std::cout << "pose> " << std::flush;
+ if (!std::getline(std::cin, line)) break;
+ if (line == "q" || line == "quit") break;
+ if (line == "open") { apply({1, 1, 1, 1, 1, 1}); continue; }
+ if (line == "close") { apply({0, 0, 0, 0, 0, 0}); continue; }
+
+ std::istringstream iss(line);
+ std::vector v; float x;
+ while (iss >> x) v.push_back(x);
+ if (v.size() != 6) { std::cout << " enter exactly 6 numbers (or open/close/q)\n"; continue; }
+ apply(v);
+ std::cout << " applied: pinky=" << v[0] << " ring=" << v[1] << " middle=" << v[2]
+ << " index=" << v[3] << " thumb_bend=" << v[4] << " thumb_rot=" << v[5] << "\n";
+ }
+ std::cout << "bye.\n";
+ return 0;
+}
diff --git a/example/hand_readtest.cpp b/example/hand_readtest.cpp
new file mode 100644
index 0000000..adff6d2
--- /dev/null
+++ b/example/hand_readtest.cpp
@@ -0,0 +1,49 @@
+/**
+ * @file hand_readtest.cpp
+ * @brief Low-level read diagnostic: send the Inspire "read angleAct" query on a
+ * serial port and dump the RAW bytes that come back. Isolates whether the
+ * hand answers position reads (writes are confirmed working).
+ *
+ * g++ -std=c++17 -Iinclude example/hand_readtest.cpp -o /tmp/hand_readtest
+ * /tmp/hand_readtest /dev/ttyUSB0 [slaveid=1] [waitms=15]
+ *
+ * Needs the port free (stop inspire_g1 / the container first).
+ */
+#include "SerialPort.h"
+#include
+#include
+#include
+#include
+
+static uint8_t checksum(const uint8_t *d, int len)
+{
+ uint8_t s = 0;
+ for (int i = 2; i < len - 1; i++) s += d[i];
+ return s;
+}
+
+int main(int argc, char **argv)
+{
+ const char *dev = argc > 1 ? argv[1] : "/dev/ttyUSB0";
+ int slave = argc > 2 ? atoi(argv[2]) : 1;
+ int waitms = argc > 3 ? atoi(argv[3]) : 15;
+
+ SerialPort sp(dev, B115200, 40); // 40ms read timeout
+ uint8_t q[9] = {0xEB, 0x90, (uint8_t)slave, 0x04, 0x11, 0x0A, 0x06, 0x0C, 0x00};
+ q[8] = checksum(q, 9);
+
+ printf("port=%s slave=%d wait=%dms (query angleAct)\n", dev, slave, waitms);
+ for (int t = 0; t < 8; t++)
+ {
+ sp.flush();
+ sp.send(q, 9);
+ usleep(waitms * 1000);
+ uint8_t buf[128];
+ size_t n = sp.recv(buf, sizeof(buf));
+ printf(" try %d: %2zu bytes:", t, n);
+ for (size_t i = 0; i < n; i++) printf(" %02X", buf[i]);
+ printf("\n");
+ usleep(150000);
+ }
+ return 0;
+}
diff --git a/example/hand_record.cpp b/example/hand_record.cpp
new file mode 100644
index 0000000..d426ace
--- /dev/null
+++ b/example/hand_record.cpp
@@ -0,0 +1,185 @@
+/**
+ * @file hand_record.cpp
+ * @brief Record & replay named hand poses (12 joint values: right 0-5, left 6-11).
+ *
+ * You sculpt a shape live (close/open individual fingers), name it, and it's saved
+ * to a file. Records the COMMANDED values (the serial read-back is unreliable on
+ * these CH340 adapters, and the hand holds whatever it's commanded anyway).
+ * Needs ./inspire_g1 running.
+ *
+ * ./hand_record [iface] interactive recorder (default iface eth0)
+ * ./hand_record play [iface] command a saved pose and hold
+ * ./hand_record list list saved poses
+ *
+ * Poses file: $HOME/DFX_inspire_service/hand_poses.txt (line: "name v0 .. v11")
+ * Per-hand joint order: pinky ring middle index thumb_bend thumb_rot. q in [0,1].
+ */
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
-
-
-