diff --git a/.gitignore b/.gitignore index c2fdc2d..540fb51 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,9 @@ photo test/photo* test/scene/* backups/* -**secret* \ No newline at end of file +**secret* +build/* +pidog.egg-info/* +**/__pycache__/* +*.egg-info/* + diff --git a/README.md b/README.md index e2a068a..5d255fe 100644 --- a/README.md +++ b/README.md @@ -95,3 +95,41 @@ E-mail: ## Credit Most sound effect are from [Zapsplat.com](https://www.zapsplat.com) + + +# Installation with one or more forked projects + +git clone the repositories from there original git location or from a fork. +Do not 'sudo python3 install.py' on any of them. + +After cloning, perform: +Setup: System Python is externally-managed (PEP 668), so I created a venv with system-site-package inheritance (needed for hardware libs like RPi.GPIO, spidev, picamera2): + +``` +>_ bash + +python3 -m venv --system-site-packages /home/pds/.venv +``` + +Editable installs — links directly to your source folders, so edits take effect immediately without reinstalling: + + + + +bash +``` +>_ bash + +/home/pds/.venv/bin/pip install -e /home/pds/robot-hat -e /home/pds/pidog -e /home/pds/vilib +``` + +Missing dependencies resolved: + +python3-pyaudio (apt) — for robot_hat.music +python3-opencv (apt) — for vilib, matches existing libopencv410 system libs +flask (pip, in-venv) — for vilib's web streaming server +Verified: import pidog, robot_hat, vilib now resolves to __init__.py, __init__.py, __init__.py respectively. + +# Python environment on raspberry PI + +In a terminal type 'source .venv/bin/activate' otherwise you will receive a 'no permission' error message. \ No newline at end of file diff --git a/basic_examples/9_rgb_control.py b/basic_examples/9_rgb_control.py index bb2a744..4bf1353 100644 --- a/basic_examples/9_rgb_control.py +++ b/basic_examples/9_rgb_control.py @@ -26,24 +26,56 @@ my_dog = Pidog() -while True: - # style="breath", color="pink" - my_dog.rgb_strip.set_mode(style="breath", color='pink') - time.sleep(3) +# while True: +# # style="breath", color="pink" +# my_dog.rgb_strip.set_mode(style="breath", color='pink') +# time.sleep(3) - # style:"listen", color=[0, 255, 255] - my_dog.rgb_strip.set_mode(style="listen", color=[0, 255, 255]) - time.sleep(3) +# # style:"listen", color=[0, 255, 255] +# my_dog.rgb_strip.set_mode(style="listen", color=[0, 255, 255]) +# time.sleep(3) - # style:"boom", color="#a10a0a" - my_dog.rgb_strip.set_mode(style="boom", color="#a10a0a") - time.sleep(3) +# # style:"boom", color="#a10a0a" +# my_dog.rgb_strip.set_mode(style="boom", color="#a10a0a") +# time.sleep(3) - # style:"boom", color="#a10a0a", brightness=0.5, bps=2.5 - my_dog.rgb_strip.set_mode(style="boom", color="#a10a0a", bps=2.5, brightness=0.5) - time.sleep(3) +# # style:"boom", color="#a10a0a", brightness=0.5, bps=2.5 +# my_dog.rgb_strip.set_mode(style="boom", color="#a10a0a", bps=2.5, brightness=0.5) +# time.sleep(3) - # close - my_dog.rgb_strip.close() - time.sleep(2) +# # close +# my_dog.rgb_strip.close() +# time.sleep(2) + + +# STYLES = ["monochromatic", +# "breath", +# "boom", +# "bark", +# "speak", +# "listen"] + +# for style in STYLES: +# my_dog.rgb_strip.set_mode(style=style, color="white") +# time.sleep(3) +# my_dog.rgb_strip.close() +# time.sleep(1) + + +my_dog.rgb_strip.set_mode(style="monochromatic", color="white") +bps = 2 +brightness = 1.0 +for i in range(5): + my_dog.rgb_strip.set_mode(style="monochromatic", color="white", bps=bps, brightness=brightness) + print(f"bps: {bps}, brightness: {brightness}") + time.sleep(0.5) + bps /= 2 + brightness -= 0.2 +bps *= 2 +brightness += 0.2 +my_dog.rgb_strip.set_mode(style="monochromatic", color="red", bps=bps, brightness=brightness) +print(f"bps: {bps}, brightness: {brightness}") +time.sleep(0.5) +my_dog.rgb_strip.close() +my_dog.close() diff --git a/docs/action_flow.md b/docs/action_flow.md new file mode 100644 index 0000000..96b5827 --- /dev/null +++ b/docs/action_flow.md @@ -0,0 +1,675 @@ +# `pidog/action_flow.py` — Detailed Documentation + +This document explains `pidog/action_flow.py` line by line and concept by concept: +what each construct is, how the pieces interact with the rest of the `pidog` +package, the exact runtime semantics of the background thread, and the sharp +edges / latent bugs in the current implementation. + +--- + +## 1. Purpose and place in the codebase + +`action_flow.py` defines **`ActionFlow`**, a *behaviour scheduler* that sits one +level above the low-level robot driver `Pidog` (`pidog/pidog.py`) and the +library of hand-written motion routines in `pidog/preset_actions.py`. + +Layering: + +``` +examples/voice_active_dog.py <- application: LLM decides "sit", "wag tail", ... + | + v +pidog/action_flow.py (ActionFlow) <- names -> motion recipes, posture management, + | queueing, idle/standby behaviour, worker thread + v +pidog/preset_actions.py <- composite routines (bark, stretch, howling, ...) + | + v +pidog/pidog.py (Pidog) <- servo buffers + servo threads (legs/head/tail) + | + v +pidog/actions_dictionary.py <- raw joint-angle tables ('stand', 'forward', ...) +``` + +`ActionFlow` gives the application a *string-based* API (`add_action('sit', +'wag tail')`) so an LLM (or any other high-level component) can trigger robot +behaviour by simply emitting action names, without knowing anything about +servo angles, posture pre-conditions or blocking semantics. + +Only consumer in this repository: `examples/voice_active_dog.py`. + +--- + +## 2. Imports (lines 1–5) + +```python +from .preset_actions import * +import threading +import time +from enum import Enum, StrEnum +import queue +``` + +| Import | Why it is there | +| --- | --- | +| `from .preset_actions import *` | Pulls every public name of `preset_actions` into this module's namespace: `scratch`, `hand_shake`, `high_five`, `pant`, `body_twisting`, `bark_action`, `shake_head`, `bark`, `push_up`, `howling`, `attack_posture`, `lick_hand`, `waiting`, `feet_shake`, `sit_2_stand`, `relax_neck`, `nod`, `think`, `recall`, `fluster`, `surprise`, `stretch`, … These are referenced unqualified inside the `OPERATIONS` lambdas. | +| `threading` | The worker thread that drains the action queue. | +| `time` | `time.time()` timestamps for the idle timer, `time.sleep()` for the polling loops. | +| `enum.Enum`, `enum.StrEnum` | The `Posetures` and `ActionStatus` enumerations. | +| `queue` | `queue.Queue` — thread-safe FIFO carrying pending action names. | + +**Side effect worth knowing:** `preset_actions.py` has no `__all__`, and it does +`import random` at module level. The star-import therefore also binds `random` +(and `sleep`, `sin`, `cos`, `pi`) inside `action_flow`. `ActionFlow.action_handler` +relies on this: it calls `random.choices(...)` / `random.randint(...)` even though +`action_flow.py` never imports `random` itself. Removing the star-import (or +adding `__all__` to `preset_actions`) would break this module with a +`NameError` unless `import random` is added. + +**Python version requirement:** `enum.StrEnum` was added in **Python 3.11**. +`pyproject.toml` declares `requires-python = ">=3.7"`, so importing this module +on 3.10 or older fails with `ImportError: cannot import name 'StrEnum'`. +This is a genuine metadata/implementation mismatch. + +--- + +## 3. `Posetures` (lines 7–10) + +```python +class Posetures(Enum): + STAND = 0 + SIT = 1 + LIE = 2 +``` + +The three gross body postures of the robot. (The name is a misspelling of +"Postures"; it is part of the public API — `examples/voice_active_dog.py` does +`from pidog.action_flow import ActionFlow, ActionStatus, Posetures` — so it +cannot be renamed without a breaking change.) + +Each posture implies a different neutral head pitch, which is why posture +changes and `head_pitch_init` are managed together (see `change_poseture`). + +--- + +## 4. `ActionStatus` (lines 12–16) + +```python +class ActionStatus(StrEnum): + STANDBY = 'standby' + THINK = 'think' + ACTIONS = 'actions' + ACTIONS_DONE = 'actions_done' +``` + +The state of the background worker thread. `StrEnum` means the members *are* +`str` instances, so `ActionStatus.STANDBY == 'standby'` is `True`. That is +essential here because `__init__` initialises the state with the raw string +`'standby'` rather than the enum member; with a plain `Enum` the comparison in +`action_handler` would silently never match and the robot would never perform +its idle behaviour. + +| Member | Meaning | Set by | +| --- | --- | --- | +| `STANDBY` | Nothing queued; the worker plays random idle "alive" motions. | `start()`, `action_handler` when the queue drains | +| `THINK` | Suspend all motion (used by the app while the LLM is generating a reply, so servo noise doesn't pollute the microphone / the dog holds a "thinking" pose). | Application, via `set_status()` | +| `ACTIONS` | There is at least one queued action; the worker drains the queue. | `add_action()` | +| `ACTIONS_DONE` | Declared but **never used** anywhere in the codebase. Dead member. | — | + +--- + +## 5. `ActionFlow` class attributes (lines 18–29) + +```python +class ActionFlow(): + SIT_HEAD_PITCH = -35 + STAND_HEAD_PITCH = 0 + HEAD_SPEED = 80 + HEAD_ANGLE = 20 + CHANGE_STATUS_SPEED = 60 + + dog_obj = None + head_yrp = [0, 0, 0] + head_pitch_init = 0 + posture = Posetures.STAND + last_actions = None +``` + +Tuning constants: + +* `SIT_HEAD_PITCH = -35` — when sitting, the body leans back, so the head servo + needs −35° of pitch compensation to keep the head looking level. +* `STAND_HEAD_PITCH = 0` — standing/lying needs no compensation. +* `HEAD_SPEED = 80` — speed used for the compensation head move. +* `HEAD_ANGLE = 20` — **unused** in this file (leftover). +* `CHANGE_STATUS_SPEED = 60` — speed for posture transitions. + +Class-level *state* (`dog_obj`, `head_yrp`, `head_pitch_init`, `posture`, +`last_actions`) exists mostly so the `OPERATIONS` lambdas can be written against +attribute names that are guaranteed to resolve. `__init__` shadows all of them +with per-instance attributes **except `last_actions`**, which therefore stays a +*class* attribute until first written. Since `run()` assigns +`self.last_actions = action`, the first write creates an instance attribute and +the class attribute is untouched — so in practice there is no cross-instance +leak, but the mutable class-level `head_yrp` list would be shared if `__init__` +did not rebind it. Prefer instance state; the class-level copies are a smell. + +* `head_yrp` — the current head target as `[yaw, roll, pitch]` in degrees. It is + passed to the preset routines that need a base orientation to animate around. + Nothing in this file ever changes it from `[0, 0, 0]`; it is effectively a hook + for a head-tracking feature (e.g. sound-direction following) that would write + into it. +* `head_pitch_init` — the current pitch compensation for the active posture. It + is passed as `pitch_comp=` to the preset routines so head animations remain + correct whether the dog is sitting or standing. +* `posture` — the posture the scheduler believes the robot is in. +* `last_actions` — the name of the last action that triggered a posture change; + used as a de-duplication guard (see §8). + +--- + +## 6. The `OPERATIONS` table (lines 31–156) + +The heart of the module: a **declarative mapping from action name → recipe**. + +```python +OPERATIONS = { + "": { + "poseture": Posetures.X, # optional: required body posture + "before": , # optional: run first + "function": , # the action itself + "after": , # optional: run last + "head_pitch": , # present once, never read + }, + ... +} +``` + +### 6.1 Key semantics (as implemented by `run()`) + +| Key | Type | Semantics | +| --- | --- | --- | +| `poseture` | `Posetures` | Pre-condition. Before the action runs, the body is transitioned into this posture (subject to the `last_actions` guard). | +| `before` | either an action name (`str`) present in `OPERATIONS`, or a `callable(self)` | Executed, then blocks until all servos are idle. | +| `function` | `callable(self)` | The action body. Executed, then blocks until all servos are idle. | +| `after` | either an action name (`str`) or `callable(self)` | Executed after `function`, then blocks. Typically used to return to a resting posture, or to repeat the action once more. | +| `head_pitch` | `int` | Present only on `"nod"`. **Never read by any code** — dead configuration. | + +All callables take exactly one argument, the `ActionFlow` instance, and are +written as `lambda self: ...`. Because they live inside the class body's dict, +they are *plain functions* stored in a dict, not bound methods, so `run()` must +pass `self` explicitly: `operation["function"](self)`. + +Two dispatch styles appear in the table: + +1. **Driver-level actions** — `lambda self: self.dog_obj.do_action('forward', speed=98)`. + `Pidog.do_action(name, step_count=1, speed=50, pitch_comp=0)` looks the name up + in `actions_dictionary.py`, gets `(angle_frames, part)` where `part` is + `'legs' | 'head' | 'tail'`, and appends the frames to the corresponding buffer + with `immediately=False` (i.e. *queued*, not interrupting). +2. **Composite routines** — `lambda self: stretch(self.dog_obj)` etc., calling + the hand-authored multi-step animations in `preset_actions.py`. These often + also play a sound via `Pidog.speak(...)`. + +### 6.2 Complete catalogue + +Locomotion (all require `STAND`, all at `speed=98` — near-maximum, gait needs it): + +| Name | Effect | +| --- | --- | +| `forward` | `do_action('forward', speed=98)` — one gait cycle forward | +| `backward` | `do_action('backward', speed=98)` | +| `turn left` | `do_action('turn_left', speed=98)` | +| `turn right` | `do_action('turn_right', speed=98)` | + +Posture: + +| Name | Effect | Posture | +| --- | --- | --- | +| `stop` | **empty dict** — no posture, no function. `run()` matches the key, finds no `function`/`before`/`after`/`poseture` and does nothing at all. It is a *documented no-op*: it exists so the LLM can emit `"stop"` without producing an `unknown action` path. Note it does **not** call `Pidog.body_stop()`; queued motion continues. | — | +| `lie` | `do_action('lie', speed=70)` | `LIE` | +| `stand` | `do_action('stand', speed=65)` | `STAND` | +| `sit` | `do_action('sit', speed=70)` | `SIT` | + +Note the double motion: for `sit`, `run()` first calls `change_poseture(SIT)` +(which itself performs `do_action('sit', speed=CHANGE_STATUS_SPEED)`) and then +the operation's own `do_action('sit', speed=70)`. The second one is effectively +a no-op re-issue at a different speed. + +Vocal / expressive: + +| Name | Recipe | Notes | +| --- | --- | --- | +| `bark` | `bark(dog, head_yrp, pitch_comp=head_pitch_init)` | Head up (+25° pitch) → `speak('single_bark_1')` → head down | +| `bark harder` | `before=attack_posture`, then `bark_action(dog, head_yrp, 'single_bark_1')`; posture `STAND` | The only entry using a **callable** `before` | +| `pant` | `pant(dog, head_yrp, pitch_comp=head_pitch_init)` | Panting sound + head bob | +| `howling` | `howling(dog)`, `after='sit'`, posture `SIT` | | +| `shake head` | `shake_head(dog, [yaw, roll, pitch + head_pitch_init])` | Compensation is folded into the third element manually rather than via a `pitch_comp` argument, because `shake_head()` has no `pitch_comp` parameter | + +Body / tricks: + +| Name | Recipe | Posture | `after` | +| --- | --- | --- | --- | +| `wag tail` | `do_action('wag_tail', speed=100)` | — | `'wag tail'` (self-reference ⇒ runs twice) | +| `stretch` | `stretch(dog)` | `SIT` | `'sit'` | +| `doze off` | `do_action('doze_off', speed=95)` | `LIE` | `'doze off'` (self-reference ⇒ twice) | +| `push up` | `push_up(dog)` | `STAND` | — | +| `twist body` | `body_twisting(dog)` | `STAND` | `'sit'` (note: posture is STAND but it ends sitting) | +| `scratch` | `scratch(dog)` | `SIT` | `'sit'` | +| `handshake` | `hand_shake(dog)` | `SIT` | `'sit'` | +| `high five` | `high_five(dog)` | `SIT` | `'sit'` | +| `lick hand` | `lick_hand(dog)` | `SIT` | — | +| `feet shake` | `feet_shake(dog)` | `SIT` | — | +| `waiting` | `waiting(dog, pitch_comp=head_pitch_init)` | — | Small random head drift; the primary idle animation | + +Emotional head animations (all `SIT`, all take `pitch_comp=head_pitch_init`): +`relax neck`, `nod`, `think`, `recall`, `fluster`, `surprise`. + +Self-referencing `after` (`wag tail`, `doze off`) is the idiom used to make a +short animation last roughly twice as long, since `run()` resolves the string +against `OPERATIONS` and calls that entry's `function` — it does **not** recurse +through `run()`, so no infinite loop occurs and the `after`'s own `after` is +ignored. + +--- + +## 7. `__init__` (lines 158–169) + +```python +def __init__(self, dog_obj): + self.dog_obj = dog_obj + self.head_yrp = [0, 0, 0] + self.head_pitch_init = 0 + self.posture = Posetures.LIE + self.thread = None + self.thread_running = False + self.thread_action_state = 'standby' + self.action_queue = queue.Queue() +``` + +* `dog_obj` — an initialised `Pidog` instance. `ActionFlow` never constructs or + owns the hardware; the application does and injects it. +* Initial posture is assumed to be **`LIE`**, which is the physical rest position + a Pidog is left in when powered off (legs folded). This differs from the class + attribute default (`STAND`) — the instance value wins. +* `thread_action_state` is initialised to the raw string `'standby'`; equal to + `ActionStatus.STANDBY` thanks to `StrEnum`. +* The queue is created here *and again* in `start()` (so a restart begins with an + empty queue). + +No thread is started by the constructor; `start()` must be called explicitly. + +--- + +## 8. Head / posture management + +### `set_head_pitch_init(self, pitch)` (lines 171–174) + +```python +self.head_pitch_init = pitch +self.dog_obj.head_move([self.head_yrp], pitch_comp=pitch, + immediately=True, speed=self.HEAD_SPEED) +``` + +Records the new pitch compensation and immediately re-issues the current head +target with it. `Pidog.head_move` takes a **list of** `[yaw, roll, pitch]` +frames, hence `[self.head_yrp]` (a one-frame list). `immediately=True` calls +`head_stop()` first, clearing any queued head frames, so the head snaps to the +new compensated orientation instead of finishing an old animation. + +`Pidog.head_rpy_to_angle` then converts yaw/roll/pitch into the three servo +angles, blending roll and pitch by the yaw ratio (the head gimbal is not +orthogonal), and adds `pitch_comp` to the pitch servo. + +### `change_poseture(self, poseture)` (lines 176–191) + +```python +if poseture == Posetures.STAND: + self.set_head_pitch_init(self.STAND_HEAD_PITCH) + if self.posture != Posetures.STAND: + sit_2_stand(self.dog_obj, speed=75) # speed > 70 + else: + self.dog_obj.do_action('stand', speed=self.CHANGE_STATUS_SPEED) +elif poseture == Posetures.SIT: + self.set_head_pitch_init(self.SIT_HEAD_PITCH) + self.dog_obj.do_action('sit', speed=self.CHANGE_STATUS_SPEED) +elif poseture == Posetures.LIE: + self.set_head_pitch_init(self.STAND_HEAD_PITCH) + self.dog_obj.do_action('lie', speed=self.CHANGE_STATUS_SPEED) + +self.posture = poseture +self.dog_obj.wait_all_done() +``` + +Key points: + +1. **Head compensation is applied first**, before the body moves, so the head is + already aimed correctly as the body transitions. +2. **Standing up is special.** Going from sit/lie to stand cannot be done with a + single target pose — the legs would slip. `sit_2_stand()` in + `preset_actions.py` moves the legs through an intermediate brace pose `L1` + before the `stand` angles. The inline + comment `# speed > 70` records a hardware constraint: below ~70 the servos + move too slowly to overcome the robot's weight and it fails to rise. If the + dog is *already* standing, the cheaper `do_action('stand')` re-levels it. +3. `LIE` uses `STAND_HEAD_PITCH` (0), not a dedicated lie value. +4. The posture field is updated **unconditionally**, even if the underlying + `do_action` failed (e.g. unknown action name — `Pidog.do_action` swallows + `KeyError` and only prints). +5. `wait_all_done()` blocks the *calling* thread (normally the worker thread) + until the legs, head and tail buffers in `Pidog` are all empty — i.e. until + the servo threads have consumed every queued frame. This is what makes + `ActionFlow` sequential despite `Pidog` being asynchronous. + +--- + +## 9. `run(self, action)` — the recipe interpreter (lines 194–228) + +```python +def run(self, action): + try: + if action in self.OPERATIONS: + operation = self.OPERATIONS[action] + # poseture + if "poseture" in operation and operation["poseture"] != None: + if self.last_actions != action: + self.last_actions = action + self.change_poseture(operation["poseture"]) + # before + if "before" in operation and operation["before"] != None: + before = operation["before"] + if before in self.OPERATIONS and self.OPERATIONS[before]["function"] != None: + self.OPERATIONS[before]["function"](self) + self.dog_obj.wait_all_done() + else: + before(self) + self.dog_obj.wait_all_done() + # function + if "function" in operation and operation["function"] != None: + operation["function"](self) + self.dog_obj.wait_all_done() + # after + if "after" in operation and operation["after"] != None: + after = operation["after"] + if after in self.OPERATIONS and self.OPERATIONS[after]["function"] != None: + self.OPERATIONS[after]["function"](self) + self.dog_obj.wait_all_done() + else: + after(self) + self.dog_obj.wait_all_done() + except Exception as e: + print(f'action error: {e}') +``` + +### Execution order + +``` +posture pre-condition -> before -> function -> after + (each stage followed by wait_all_done(), i.e. fully blocking) +``` + +### Behaviour details + +* **Unknown action names are silently ignored.** `if action in self.OPERATIONS` + with no `else`. This matters because the LLM in `voice_active_dog.py` is free + to hallucinate action names, and because the standby list contains + `'feet_left_right'`, which is *not* a key in `OPERATIONS` (see §10). +* **The posture guard is `last_actions`, not `posture`.** The commented-out line + `# if self.posture != operation["poseture"]:` shows the original intent: skip + the transition if already in the right posture. The shipped code instead skips + it when *the same action is repeated back-to-back*. Consequences: + * Repeating `sit` twice in a row does not re-issue the sit transition — good. + * Alternating `handshake`, `high five` (both `SIT`) re-runs the full sit + transition every time — wasteful, and visibly jerky. + * `last_actions` is only updated inside this branch, so actions without a + `poseture` key (e.g. `bark`, `pant`, `wag tail`) never update it. After + `sit` → `bark` → `sit`, the second `sit` is still considered a repeat and + the posture transition is skipped, even though `bark` may have disturbed the + pose. This is a latent bug, though usually harmless. +* **String-vs-callable dispatch for `before`/`after`** is done with + `if before in self.OPERATIONS`. Two hazards: + * `X in dict` on a *callable* is a hash lookup of a function object — safe, + returns `False`, falls through to `before(self)`. Fine. + * `self.OPERATIONS[before]["function"]` raises **`KeyError: 'function'`** if the + referenced entry has no `function` key. Today the only such entry is + `"stop"`, and nothing references it as `before`/`after`, so the bug is + unreachable — but adding `"after": "stop"` anywhere would trip it. The + `KeyError` would be caught by the outer `except` and printed as + `action error: 'function'`. +* **Blanket `except Exception`.** Any hardware error, I2C failure, missing sound + file or programming mistake inside an action becomes a printed line and the + scheduler carries on. Good for robot uptime, bad for debuggability — failures + are invisible to callers, and `run()` never signals success/failure. +* **`run()` is synchronous and blocking.** It returns only when every servo + buffer is drained. It is normally called from the worker thread, but nothing + prevents an application from calling it directly from the main thread — doing + so concurrently with a running worker would interleave two action recipes on + the same servo buffers, with no locking. + +--- + +## 10. `action_handler(self)` — the worker loop (lines 230–259) + +```python +def action_handler(self): + standby_actions = ['waiting', 'feet_left_right'] + standby_weights = [1, 0.3] + + action_interval = 5 # seconds + last_action_time = time.time() + + while self.thread_running: + if self.thread_action_state == ActionStatus.STANDBY: + if time.time() - last_action_time > action_interval: + choice = random.choices(standby_actions, standby_weights)[0] + self.run(choice) + last_action_time = time.time() + action_interval = random.randint(2, 6) + elif self.thread_action_state == ActionStatus.THINK: + pass + elif self.thread_action_state == ActionStatus.ACTIONS: + _action = self.action_queue.get() + try: + self.run(_action) + except Exception as e: + print(f'action error: {e}') + + if self.action_queue.empty(): + self.thread_action_state = ActionStatus.STANDBY + last_action_time = time.time() + + time.sleep(0.5) + + time.sleep(0.01) +``` + +This is the body of the thread created in `start()`. It polls the state at +~100 Hz. + +### `STANDBY` — idle "alive" behaviour + +Every `action_interval` seconds (initially 5, then a random 2–6) it plays a +random idle motion, weighted 1 : 0.3 between `'waiting'` and +`'feet_left_right'`. `random.choices` accepts unnormalised weights, so the +effective probabilities are 1/1.3 ≈ 77 % and 0.3/1.3 ≈ 23 %. + +**`'feet_left_right'` is not a key in `OPERATIONS`.** `run()` therefore does +nothing for it. The practical effect is that ~23 % of idle ticks are silent +no-ops — the dog just pauses. The intended action almost certainly was +`'feet shake'` (`feet_shake` in `preset_actions.py`). This is a real bug, and a +good example of why `run()` silently ignoring unknown names is dangerous. + +Note also that idle actions are *not* affected by the `last_actions` guard, +because `'waiting'` has no `poseture` key. + +### `THINK` — suspended + +Explicit `pass`. The loop keeps spinning at 100 Hz but issues no motion and, +importantly, **does not drain the queue**. The idle timer is not reset either, +so on the first transition back to `STANDBY` an idle action fires almost +immediately (`time.time() - last_action_time` is already large). + +### `ACTIONS` — draining the queue + +`self.action_queue.get()` is a **blocking** call with no timeout. If the state is +`ACTIONS` but the queue is empty, the worker thread parks inside `get()` +indefinitely; `self.thread_running = False` will not wake it, so `stop()` would +hang on `thread.join()`. In practice the state is only set to `ACTIONS` by +`add_action()`, which always enqueues at least one item first, and the state is +flipped back to `STANDBY` as soon as the queue drains — so the window is small +but real: if `add_action()` is called concurrently and the state is re-set to +`ACTIONS` just after the worker's emptiness check, or if `set_status(ACTIONS)` +is called by hand, the thread can block forever. + +After each action there is a deliberate `time.sleep(0.5)` — a short pause +between chained actions so the sequence reads as distinct gestures rather than +one continuous blur. + +The trailing `time.sleep(0.01)` is the poll interval for all states. + +--- + +## 11. Public control API (lines 261–283) + +### `add_action(self, *actions)` + +```python +for action in actions: + self.action_queue.put(action) +self.thread_action_state = ActionStatus.ACTIONS +``` + +Enqueues one or more action names and switches the worker into `ACTIONS`. +Enqueue-then-set-state is the correct order (the reverse would expose the +blocking-`get()` window described above). Names are not validated. + +Usage in `voice_active_dog.py`: `self.action_flow.add_action(*actions)` where +`actions` is the list the LLM returned in its JSON response. + +### `set_status(self, status)` + +Direct assignment of the worker state. Used by the application to enter `THINK` +while the LLM is generating and `STANDBY` afterwards. No validation, no locking +(fine in CPython for a single attribute assignment). + +### `wait_actions_done(self)` + +```python +while self.thread_action_state != ActionStatus.STANDBY: + time.sleep(0.01) +``` + +Blocks the caller until the worker returns to `STANDBY` — i.e. until the queue +is empty and the last action finished. Note it waits for the **state**, not the +queue, so it also blocks forever if the state is `THINK`. Callers must therefore +leave `THINK` before waiting. 100 Hz busy-poll rather than an `Event`/`join`. + +### `start(self)` + +```python +self.thread_running = True +self.thread_action_state = ActionStatus.STANDBY +self.action_queue = queue.Queue() +self.thread = threading.Thread(name="action_handler", target=self.action_handler) +self.thread.start() +``` + +Resets state, discards any previously queued actions, and launches the worker. +The thread is **non-daemon**, so an application that forgets `stop()` will not +exit until the thread ends. Calling `start()` twice leaks the first thread (the +old thread keeps running because `thread_running` was set back to `True`). + +### `stop(self)` + +```python +self.thread_running = False +if self.thread != None: + self.thread.join() +``` + +Requests shutdown and waits. Shutdown latency is bounded by the *current +action*: `run()` blocks until all servo frames are consumed, so `stop()` can take +seconds. If the worker is parked in `action_queue.get()`, `join()` never returns +(see §10). + +--- + +## 12. End-to-end example (from `examples/voice_active_dog.py`) + +```python +from pidog.pidog import Pidog +from pidog.action_flow import ActionFlow, ActionStatus, Posetures + +dog = Pidog() +action_flow = ActionFlow(dog) + +action_flow.set_status(ActionStatus.STANDBY) +action_flow.start() # worker begins idle "waiting" motions +action_flow.change_poseture(Posetures.SIT) + +# user speaks -> app suspends motion while the LLM thinks +action_flow.set_status(ActionStatus.THINK) + +# LLM returns {"actions": ["wag tail", "bark"], "answer": "..."} +action_flow.add_action("wag tail", "bark") # state -> ACTIONS +action_flow.wait_actions_done() # blocks until back in STANDBY + +action_flow.change_poseture(Posetures.SIT) +action_flow.stop() +``` + +What happens for `add_action("wag tail", "bark")`: + +1. `"wag tail"`, `"bark"` are pushed onto the FIFO; state becomes `ACTIONS`. +2. Worker pops `"wag tail"`. No `poseture` key ⇒ no posture change. + No `before`. `function` ⇒ `do_action('wag_tail', speed=100)`, then + `wait_all_done()`. `after` is `"wag tail"` ⇒ the same function runs a second + time, then `wait_all_done()`. +3. Queue not empty ⇒ stay in `ACTIONS`; sleep 0.5 s. +4. Worker pops `"bark"`. No posture, no before. `function` ⇒ + `bark(dog, [0,0,0], pitch_comp=head_pitch_init)` — head lunge plus the + the `single_bark_1` sound. `wait_all_done()`. No `after`. +5. Queue now empty ⇒ state back to `STANDBY`, idle timer reset, sleep 0.5 s. +6. `wait_actions_done()` in the app returns. + +--- + +## 13. Concurrency model summary + +| Thread | Runs | Touches | +| --- | --- | --- | +| Application / main | `add_action`, `set_status`, `wait_actions_done`, `change_poseture`, `start`, `stop` | `thread_action_state`, `action_queue`, and (via `change_poseture`) the servo buffers | +| `action_handler` worker | `run()` and everything below it | servo buffers, `posture`, `head_pitch_init`, `last_actions`, `thread_action_state` | +| `Pidog` servo threads (legs/head/tail) | consume the frame buffers | hardware | + +Synchronisation primitives actually used: only `queue.Queue` (for the action +names) and the locks *inside* `Pidog` (for the buffers). `thread_action_state`, +`posture`, `head_pitch_init` and `head_yrp` are unguarded shared state; the +design relies on the GIL making single attribute reads/writes atomic and on the +application not issuing motion commands while the worker is active. Notably +`change_poseture()` called from the main thread **can** race with an action +running on the worker thread. + +--- + +## 14. Known issues / improvement candidates + +1. `'feet_left_right'` in `standby_actions` is not a defined operation — dead + idle branch (~23 % of idle ticks do nothing). Likely meant `'feet shake'`. +2. `StrEnum` requires Python ≥ 3.11 while `pyproject.toml` says `>= 3.7`. +3. `run()` silently ignores unknown action names — no logging, so issue #1 is + invisible at runtime. +4. Posture guard keys off `last_actions` (last action name) instead of the + current `posture`, causing both redundant transitions and skipped ones. +5. `self.OPERATIONS[before]["function"]` assumes a `"function"` key exists — + `KeyError` for entries like `"stop"`. +6. `action_queue.get()` has no timeout ⇒ possible permanent block, which turns + `stop()`/`join()` into a hang. +7. `wait_actions_done()` and the worker loop busy-poll; `queue.join()` / + `threading.Event` would be cheaper and race-free. +8. Dead code: `HEAD_ANGLE`, `ActionStatus.ACTIONS_DONE`, the `head_pitch` key on + `"nod"`, the class-level state duplicates. +9. `"stop"` does not stop anything — it does not call `Pidog.body_stop()`. +10. `run()` depends on `random` being leaked in by + `from .preset_actions import *`; an explicit `import random` would be safer. +11. Spelling: `Posetures` / `poseture` (public API, so renaming is breaking). diff --git a/docs/pidog.md b/docs/pidog.md new file mode 100644 index 0000000..e1fd71b --- /dev/null +++ b/docs/pidog.md @@ -0,0 +1,1104 @@ +# `pidog/pidog.py` — Detailed Documentation + +This document explains `pidog/pidog.py` in depth: every module-level construct, +every attribute and method of the `Pidog` class, the threading/IPC model, the +kinematics maths, and the sharp edges / latent bugs in the current +implementation. + +Companion document: [`action_flow.md`](./action_flow.md), which documents the +behaviour scheduler layered on top of this driver. + +--- + +## 1. Purpose and place in the codebase + +`pidog.py` defines **`Pidog`** — the *hardware abstraction layer* for the +SunFounder Pidog robot. It is the single object that owns every peripheral and +every background thread, and it is what every other module in the package is +ultimately talking to. + +``` +examples/*.py, pidog-control <- applications + | + v +pidog/action_flow.py (ActionFlow) <- named behaviours, queueing, postures + | + v +pidog/preset_actions.py <- composite animations (bark, stretch, …) + | + v +pidog/pidog.py (Pidog) <<< THIS FILE >>> + | | | + | | +-- pidog/actions_dictionary.py (ActionDict) -> walk.py / trot.py + | | raw joint-angle frames per named action + | +-- pidog/sh3001.py (IMU), rgb_strip.py, sound_direction.py, dual_touch.py + +-- robot_hat (Robot, Pin, Ultrasonic, Music, utils, I2C) -> servos / I²C / audio +``` + +Responsibilities of `Pidog`: + +1. **Initialise all hardware** — 12 servos in three groups (legs/head/tail), + IMU, RGB chest strip, dual touch sensors, sound-direction sensor, audio, + ultrasonic. +2. **Provide non-blocking motion primitives** — `legs_move`, `head_move`, + `tail_move`, `do_action`. These *append frames to buffers*; dedicated + threads drain the buffers into servo writes. +3. **Provide the synchronisation primitives** callers use to make that + asynchronous model look sequential — `wait_all_done`, `is_all_done`, + `body_stop`. +4. **Kinematics** — convert body pose (x, y, z + roll/pitch/yaw) into eight leg + joint angles, and convert head yaw/roll/pitch into three head servo angles. +5. **Sensors** — expose ultrasonic distance, IMU-derived `pitch`/`roll`, battery + voltage. +6. **Audio** — `speak` / `speak_block`. +7. **Calibration** — persist servo offsets to `~/.config/pidog/pidog.conf`. + +--- + +## 2. Module header + +### 2.1 Imports (lines 1–15) + +```python +import os, sys +from time import sleep, time +from multiprocessing import Process, Value, Lock +import threading +import numpy as np +from math import pi, sin, cos, sqrt, acos, atan2, atan +from robot_hat import Robot, Pin, Ultrasonic, utils, Music, I2C +from .sh3001 import Sh3001 +from .rgb_strip import RGBStrip +from .sound_direction import SoundDirection +from .dual_touch import DualTouch +import warnings +warnings.filterwarnings("ignore") # ignore warnings for pygame # not work +``` + +| Import | Used for | +| --- | --- | +| `multiprocessing.Value`, `Lock` | Shared ultrasonic distance (`Value('f', -1.0)`) and its lock. `Process` is imported but **no longer used** — the "sensory process" is now a `threading.Thread` (see §9). The shared-memory `Value` is a leftover from the process-based design; it still works fine as a plain float holder. | +| `robot_hat.Robot` | Servo group driver: holds pins, offsets, speed limiting (`max_dps`) and the config DB. | +| `robot_hat.Pin` | GPIO for the ultrasonic trigger/echo (`D0`/`D1`) and touch pins. | +| `robot_hat.Ultrasonic` | HC-SR04-style distance sensor. | +| `robot_hat.Music` | Audio playback (pygame based). | +| `robot_hat.utils` | `reset_mcu()`, `run_command()`, `get_battery_voltage()`. | +| `robot_hat.I2C` | Imported, **unused** in this file. | +| `Sh3001` | 6-axis IMU driver. | +| `RGBStrip` | 11-LED chest strip (IS31FL-style controller at I²C `0x74`). | +| `SoundDirection` | Sound-direction-of-arrival sensor. | +| `DualTouch` | Two head touch pads on `D2`/`D3`. | +| `numpy` | Matrix maths for the body-pose kinematics. | + +The `warnings.filterwarnings("ignore")` call is an attempt to silence pygame's +import chatter; the author's own comment `# not work` records that it doesn't. + +### 2.2 Servo map (lines 17–39) + +The ASCII diagram documents the physical wiring: + +``` + 4, + 5, '6' + | + 3,2 --[ ]-- 7,8 + [ ] + 1,0 --[ ]-- 10,11 + | + '9' +``` + +* **Legs** — `[2, 3, 7, 8, 0, 1, 10, 11]`, i.e. pairs of *(hip, knee)* for + left-front, right-front, left-hind, right-hind. Every 8-element leg angle + list in `actions_dictionary.py` follows exactly this order. +* **Head** — `[4, 6, 5]` = *(yaw, roll, pitch)*. +* **Tail** — `[9]`. + +(The comment block labels the first four leg entries as "left front leg" twice +and "right front leg" twice; read them as hip/knee pairs.) + +### 2.3 User & config-file discovery (lines 41–45) + +```python +is_run_with_root = (os.geteuid() == 0) +User = os.popen('echo ${SUDO_USER:-$LOGNAME}').readline().strip() +UserHome = os.popen('getent passwd %s | cut -d: -f 6' % User).readline().strip() +config_file = '%s/.config/pidog/pidog.conf' % UserHome +``` + +Pidog examples are usually run with `sudo` (needed for I²C/audio on some +setups). Running as root would make `~` resolve to `/root`, so calibration +offsets would be written to the wrong place. The `${SUDO_USER:-$LOGNAME}` trick +recovers the *invoking* user, and `getent passwd | cut -d: -f6` yields that +user's home directory. `config_file` is passed as `db=` to every `Robot` +instance and to `Sh3001`, so **all calibration state lives in one file**: +`~/.config/pidog/pidog.conf`. + +Note this is evaluated **at import time**, spawning two subshells, and +`SOUND_DIR` (§3) is derived from it as `f"{UserHome}/pidog/sounds/"` — i.e. the +sounds are expected in a *clone of the repo in the user's home directory*, not +in the installed package. + +### 2.4 Coloured logging helpers (lines 47–72) + +ANSI colour constants plus `print_color()` and the four wrappers used +throughout the file: + +| Helper | Colour | Used for | +| --- | --- | --- | +| `info` | white | user-facing status (`'Quit'`, `'Please wait'`) | +| `debug` | gray | init progress (`"robot_hat init ... "` / `"done"`) | +| `warn` | yellow | recoverable problems (`No sound found for ...`) | +| `error` | red | failures (init failure, thread exceptions) | + +There is no `logging` module usage — everything goes to stdout unconditionally. + +### 2.5 `compare_version` and the numpy-2 shim (lines 75–85) + +```python +def compare_version(original_version, object_version): + or_v = tuple(int(val) for val in original_version.split('.')) + ob_v = tuple(int(val) for val in object_version.split('.')) + return (or_v >= or_v) # <-- BUG: compares or_v with itself + +if compare_version(np.__version__, '2.0.0'): + def numpy_mat(data): + return np.asmatrix(data) +else: + def numpy_mat(data): + return numpy_mat(data) # <-- BUG: infinite recursion +``` + +Intent: NumPy 2 removed `np.mat`, so use `np.asmatrix` on ≥ 2.0.0 and `np.mat` +on older versions. + +Two defects, which happen to cancel out: + +1. `return (or_v >= or_v)` compares the tuple with **itself** — it is always + `True`, regardless of the installed NumPy version. (`ob_v` is computed and + discarded. The intended expression is `or_v >= ob_v`.) +2. Because of (1), the `else` branch is dead. That is fortunate, since it + defines `numpy_mat` as a function that calls *itself* — invoking it would + raise `RecursionError`. The intended body was `return np.mat(data)`. + +Net effect: `numpy_mat` is always `np.asmatrix`, which works on NumPy ≥ 1.x as +well, so the shim is harmless in practice — but both lines are wrong. + +`numpy_mat` is used for `BODY_STRUCT`, `pose`, `leg_point_struc`, and the +rotation matrices, i.e. everywhere the code relies on `*` meaning *matrix +multiply* rather than element-wise multiply. That is the only reason +`np.matrix` (a deprecated type) is used at all. + +--- + +## 3. `Pidog` class constants (lines 89–124) + +### Mechanical structure + +| Constant | Value | Meaning | +| --- | --- | --- | +| `LEG` | 42 | Upper leg (thigh) length, mm | +| `FOOT` | 76 | Lower leg (shank) length, mm | +| `BODY_LENGTH` | 117 | Front-to-hind hip distance, mm | +| `BODY_WIDTH` | 98 | Left-to-right hip distance, mm | +| `BODY_STRUCT` | 3×4 matrix | Hip coordinates in the body frame, one column per leg, ordered LF, RF, LH, RH: `[±W/2, ±L/2, 0]`. Transposed (`.T`) so each **column** is a point, which is what the rotation matrix multiplication expects. | +| `SOUND_DIR` | `~/pidog/sounds/` | Where `speak()` looks for audio | + +`LEG` and `FOOT` are the two link lengths of the 2-link planar arm solved by +`coord2polar` / `fieldcoord2polar` (§12). + +### Servo speed limits (degrees per second) + +```python +HEAD_DPS = 300 +LEGS_DPS = 428 +TAIL_DPS = 500 +``` + +Assigned to `Robot.max_dps` after construction, so `robot_hat` rate-limits each +group independently. The commented-out block above them (`LEGS_DPS = 350`) +records earlier, more conservative tuning. Legs are fastest-but-one because gait +frames must be issued quickly; the tail is the lightest load, hence 500. + +### PID constants + +```python +KP = 0.033 +KI = 0.0 +KD = 0.0 +``` + +Used only by `set_rpy(..., pid=True)` for IMU-based self-levelling. Only the +proportional term is active — integral and derivative are disabled. `KP` is +small because the loop runs at whatever rate the caller polls, and overshoot on +a 12-servo body is very visible. + +### Pin defaults and head limits + +```python +DEFAULT_LEGS_PINS = [2, 3, 7, 8, 0, 1, 10, 11] +DEFAULT_HEAD_PINS = [4, 6, 5] # yaw, roll, pitch +DEFAULT_TAIL_PIN = [9] + +HEAD_PITCH_OFFSET = 45 + +HEAD_YAW_MIN, HEAD_YAW_MAX = -90, 90 +HEAD_ROLL_MIN, HEAD_ROLL_MAX = -70, 70 +HEAD_PITCH_MIN, HEAD_PITCH_MAX = -45, 30 +``` + +`HEAD_PITCH_OFFSET = 45` is a **mechanical** offset: the pitch servo's zero +position is 45° away from "head level". It is added in two places — at init +(`head_init_angles[2] += HEAD_PITCH_OFFSET`) and inside `_head_action_thread` +just before writing to the servo — so all *public* head angles are expressed in +the natural "0 = level" frame. The asymmetric pitch range (−45…+30) reflects +that the head can droop further than it can lift before hitting the body. + +--- + +## 4. `__init__` (lines 127–264) + +Signature: + +```python +def __init__(self, leg_pins=DEFAULT_LEGS_PINS, head_pins=DEFAULT_HEAD_PINS, + tail_pin=DEFAULT_TAIL_PIN, + leg_init_angles=None, head_init_angles=None, tail_init_angle=None): +``` + +### 4.1 MCU reset (lines 131–132) + +```python +utils.reset_mcu() +sleep(0.2) +``` + +Hard-resets the robot_hat co-processor that generates the servo PWM, so a +previous crashed run cannot leave stale servo state. The 200 ms sleep is the +MCU boot time; skipping it makes the subsequent I²C writes fail. + +### 4.2 Action dictionary (lines 134–135) + +```python +from .actions_dictionary import ActionDict +self.actions_dict = ActionDict() +``` + +The import is **deliberately local** rather than at module top: `actions_dictionary.py` +does `from .pidog import Pidog` (it calls the `Pidog.legs_angle_calculation` +classmethod to precompute gait angles). A top-level import here would be a +circular import; deferring it to call time breaks the cycle. + +`ActionDict` subclasses `dict` and overrides `__getitem__` as +`eval("self.%s" % item.replace(" ", "_"))`, so `actions_dict['lie']` evaluates +the `lie` **property**, which returns a tuple `(frames, part)` where `part ∈ +{'legs', 'head', 'tail'}`. That is why `do_action` unpacks +`actions, part = self.actions_dict[action_name]`, and why an unknown name raises +`KeyError`… actually an `AttributeError` wrapped by `eval`; see §14. + +### 4.3 Pose / kinematics state (lines 137–153) + +```python +self.body_height = 80 +self.pose = numpy_mat([0.0, 0.0, self.body_height]).T # target position vector +self.rpy = np.array([0.0, 0.0, 0.0]) * pi / 180 # radians +self.leg_point_struc = numpy_mat([...]).T # foot targets, body frame +self.pitch = 0 # measured, from IMU +self.roll = 0 # measured, from IMU +self.roll_last_error = 0 +self.roll_error_integral = 0 +self.pitch_last_error = 0 +self.pitch_error_integral = 0 +self.target_rpy = [0, 0, 0] +``` + +Important distinction: + +* `self.rpy` — the **commanded** body orientation, in **radians**. +* `self.roll` / `self.pitch` — the **measured** orientation from the IMU, in + **degrees**, updated by `_imu_thread`. +* `self.target_rpy` — the setpoint the PID branch of `set_rpy` drives towards, + in degrees. + +Note `self.leg_point_struc` is initialised here but `set_legs()` writes +`self.legpoint_struc` (no underscore between "leg" and "point"), and +`pose2coords()` reads `self.legpoint_struc`. They are **two different +attributes**; the initialised one is never read. Consequence: calling +`pose2coords()` / `pose2legs_angle()` before `set_legs()` raises +`AttributeError: 'Pidog' object has no attribute 'legpoint_struc'`. See §14. + +### 4.4 Default initial angles (lines 155–163) + +```python +if leg_init_angles == None: + leg_init_angles = self.actions_dict['lie'][0][0] +if head_init_angles == None: + head_init_angles = [0, 0, self.HEAD_PITCH_OFFSET] +else: + head_init_angles[2] += self.HEAD_PITCH_OFFSET +if tail_init_angle == None: + tail_init_angle = [0] +``` + +* Legs default to the **lie** pose `[45, -45, -45, 45, 45, -45, -45, 45]` — the + safe, low-torque folded position to power up in. +* Head pitch is pre-offset so the head is level at boot. Note the `else` + branch **mutates the caller's list in place** — passing the same list to two + `Pidog` instances would double-apply the offset. +* Comparisons use `== None` rather than `is None` throughout the file. + +### 4.5 Servo groups (lines 167–205) + +```python +self.legs = Robot(pin_list=leg_pins, name='legs', init_angles=leg_init_angles, + init_order=[0, 2, 4, 6, 1, 3, 5, 7], db=config_file) +self.head = Robot(pin_list=head_pins, name='head', init_angles=head_init_angles, db=config_file) +self.tail = Robot(pin_list=tail_pin, name='tail', init_angles=tail_init_angle, db=config_file) +``` + +`init_order=[0, 2, 4, 6, 1, 3, 5, 7]` makes the legs power up **all hips first, +then all knees**. Energising a knee before its hip would make the leg kick out +and possibly tip the robot over. + +`name=` selects the section in `pidog.conf` where that group's calibration +offsets live; `db=config_file` points all three at the same file. + +Then per-group state is created: + +| Attribute | Purpose | +| --- | --- | +| `legs_action_buffer`, `head_action_buffer`, `tail_action_buffer` | FIFO lists of angle frames waiting to be written to servos | +| `legs_thread_lock`, `head_thread_lock`, `tail_thread_lock` | Guard the corresponding buffer | +| `leg_current_angles`, `head_current_angles`, `tail_current_angles` | Last frame handed to the servos; read by `preset_actions` (e.g. `feet_shake` copies `leg_current_angles` to build a relative motion) | +| `legs_speed`, `head_speed`, `tail_speed` | Current speed (0–100) — note this is **per group, not per frame**: the last `*_move()` call's speed applies to whatever is in the buffer when the thread gets to it | +| `legs_actions_coords_buffer` | Created, never used | + +Failure of this block raises `OSError("rotbot_hat I2C init failed…")` (sic) — a +`Pidog` without servos is not usable, so this is the one fatal init step. + +### 4.6 Optional peripherals (lines 207–254) + +Each peripheral is initialised in its own `try/except` that prints `fail` and +**continues**. The pattern is deliberate: a Pidog missing a chest strip or a +sound-direction board should still walk. + +| Block | Creates | Registers thread | On failure | +| --- | --- | --- | --- | +| IMU | `self.imu = Sh3001(db=config_file)`, offsets, `accData`, `gyroData`, `imu_fail_count` | `"imu"` | prints `fail`; `self.imu` never assigned | +| RGB strip | `self.rgb_strip = RGBStrip(addr=0x74, nums=11)`, set to `breath`/`black`, `rgb_thread_run = True` | `"rgb"` | prints `fail` | +| Dual touch | `self.dual_touch = DualTouch('D2', 'D3')`, `self.touch = 'N'` | — (polled by the app) | bare `except:` | +| Sound direction | `self.ears = SoundDirection()` | — (polled by the app) | bare `except:` | +| Audio | `self.music = Music()` | — | bare `except:` | + +Note the inconsistency: the first two catch only `OSError`, the last three use a +bare `except:` (which also swallows `KeyboardInterrupt`/`SystemExit`). + +`self.thread_list` is the registry that later drives `action_threads_start()` +and `close()` — a peripheral that failed to init simply never appears in it, so +no thread is started and no join is attempted for it. + +### 4.7 Ultrasonic + startup (lines 256–264) + +```python +self.distance = Value('f', -1.0) +self.sensory_process = None +self.sensory_lock = Lock() + +self.exit_flag = False +self._sensory_exit_flag = False +self.action_threads_start() +self.sensory_process_start() +``` + +`-1.0` is the "no reading yet / invalid" sentinel for distance. Then all +background workers are started — **the constructor leaves five to six threads +running**. + +--- + +## 5. Threading model + +This is the single most important thing to understand about `Pidog`. + +| Thread | Target | Daemon | Loop exit condition | Touches | +| --- | --- | --- | --- | --- | +| `legs_thread` | `_legs_action_thread` | yes | `exit_flag` | `legs_action_buffer`, `legs.servo_move` | +| `head_thread` | `_head_action_thread` | yes | `exit_flag` | `head_action_buffer`, `head.servo_move` | +| `tail_thread` | `_tail_action_thread` | yes | `exit_flag` | `tail_action_buffer`, `tail.servo_move` | +| `rgb_strip_thread` | `_rgb_strip_thread` | yes | `rgb_thread_run` | `rgb_strip.show()` | +| `imu_thread` | `_imu_thread` | yes | `exit_flag` | `accData`, `gyroData`, `pitch`, `roll` | +| `sensory_thread` | `sensory_process_work` | **no** | (spawns the next one and returns) | creates the ultrasonic device | +| `ultrasonic_thread` | `_ultrasonic_thread` | **no** | `_sensory_exit_flag` | `self.distance` | + +**Producer/consumer contract:** the caller's thread appends frames +(`legs_move`, `head_move`, `tail_move`, `do_action`); the group thread pops +frames and blocks inside `robot_hat`'s `servo_move`, which interpolates to the +target at `max_dps` limited by `*_speed`. "Action finished" therefore means +"buffer empty", which is exactly what `is_*_done()` reports and +`wait_*_done()` polls. + +Consequence worth internalising: **`legs_move()` returns immediately**. Any +sequential-looking code must call `wait_all_done()` (this is what +`preset_actions` and `ActionFlow.run()` do after every step). + +--- + +## 6. Lifecycle methods + +### `action_threads_start()` (lines 358–380) + +Creates and starts one daemon thread per entry in `thread_list`. It is called +by `__init__` and again by `close()` if the threads had been stopped. Because +the threads are daemons, a Python process that forgets to call `close()` can +still exit. + +Calling it twice without setting `exit_flag` first would start **duplicate** +threads competing for the same buffers; nothing guards against that. + +### `close_all_thread()` (lines 270–271) + +One-liner: `self.exit_flag = True`. Stops legs/head/tail/imu loops (but *not* +the RGB loop, which watches `rgb_thread_run`, nor the ultrasonic loop, which +watches `_sensory_exit_flag`). + +### `close()` (lines 273–327) + +The full shutdown sequence: + +```python +signal.signal(signal.SIGINT, handler) # Ctrl-C during shutdown -> "Please wait" +signal.signal(signal.SIGALRM, _handle_timeout) +signal.alarm(5) # hard 5 s budget for shutdown +``` + +1. **SIGINT is swallowed** during shutdown so an impatient second Ctrl-C cannot + leave the servos energised in a bad pose. +2. **SIGALRM after 5 s** raises `TimeoutError` inside whatever is currently + executing, so a wedged `join()` cannot hang the process forever. The + exception is caught by the outer `except Exception` and reported as + `Close error: function timeout`. Note `signal.alarm()` only works on the main + thread of the main interpreter. +3. If the threads were already stopped (`exit_flag == True`), they are + **restarted** — otherwise `stop_and_lie()` would enqueue frames that nobody + consumes and `wait_all_done()` would block until the alarm fires. +4. `stop_and_lie()` — return to the safe folded pose. +5. `close_all_thread()` — signal legs/head/tail/imu to stop. +6. Close `dual_touch`, `ears`, `ultrasonic` if present (`hasattr` guards, + because those inits are allowed to fail). +7. `join()` the three servo threads, then RGB (after `rgb_thread_run = False` + and `rgb_strip.close()`), then IMU, then the sensory thread with + `_sensory_exit_flag = True` and `join(timeout=1)`. + +The trailing commented-out `finally:` block (restoring the default SIGINT +handler, cancelling the alarm, `sys.exit(0)`) means **the alarm is left armed +and SIGINT stays hijacked after `close()` returns**. If the process lives on +for more than the remaining alarm time, a stray `TimeoutError` can surface at an +arbitrary point. Re-enabling `signal.alarm(0)` would be the fix. + +--- + +## 7. The servo worker threads + +### `_legs_action_thread` (lines 383–396) + +```python +while not self.exit_flag: + try: + with self.legs_thread_lock: + self.leg_current_angles = list.copy(self.legs_action_buffer[0]) + # lock released before the slow part + self.legs.servo_move(self.leg_current_angles, self.legs_speed) + with self.legs_thread_lock: + self.legs_action_buffer.pop(0) + except IndexError: + sleep(0.001) + except Exception as e: + error(f'\r_legs_action_thread Exception:{e}') + break +``` + +Design notes: + +* **`IndexError` is the idle signal.** Rather than a condition variable, the + thread indexes `[0]` on an empty list and treats the exception as "nothing to + do", sleeping 1 ms. Simple, but it means an empty buffer costs a 1 kHz + exception-throwing spin. +* **The lock is held only around the list access**, never across + `servo_move()` (which blocks for the whole interpolated motion). This is what + lets `legs_stop()` clear the buffer mid-motion. +* **Legs pop *after* the move; head and tail pop *before*.** This asymmetry is + significant: for legs, `is_legs_done()` (buffer empty) becomes true only once + the last frame has physically finished, whereas for head/tail the buffer + empties one frame *before* the motion completes. So `wait_head_done()` can + return while the head is still moving — several routines in + `preset_actions.py` compensate with explicit `sleep()`s. +* A clear during a move leaves the just-completed frame in flight and then + `pop(0)` removes *someone else's* frame if new frames arrived in the interim — + a small race window inherent to the pop-after-move ordering. +* Any non-`IndexError` exception **kills the thread permanently** (`break`); the + robot then silently stops responding to leg commands. + +### `_head_action_thread` (lines 399–416) + +Same shape, plus the head-specific transformation applied *at write time*: + +```python +_angles[0] = self.limit(self.HEAD_YAW_MIN, self.HEAD_YAW_MAX, _angles[0]) +_angles[1] = self.limit(self.HEAD_ROLL_MIN, self.HEAD_ROLL_MAX, _angles[1]) +_angles[2] = self.limit(self.HEAD_PITCH_MIN, self.HEAD_PITCH_MAX, _angles[2]) +_angles[2] += self.HEAD_PITCH_OFFSET +``` + +Clamping happens **here**, not in `head_move()`, so callers can enqueue +out-of-range values freely and the hardware is still protected. Because the +clamp is applied to the copy `_angles`, `head_current_angles` keeps the +*unclamped* value — so `head_current_angles` is the commanded, not the actual, +orientation. + +### `_tail_action_thread` (lines 419–431) + +The simplest of the three: pop, then `servo_move`. No clamping. + +### `_rgb_strip_thread` (lines 434–444) + +```python +while self.rgb_thread_run: + try: + self.rgb_strip.show() + self.rgb_fail_count = 0 + except Exception as e: + self.rgb_fail_count += 1 + sleep(0.001) + if self.rgb_fail_count > 10: + error(...); break +``` + +`RGBStrip.show()` renders one animation frame over I²C and paces itself +internally. The failure counter tolerates up to 10 *consecutive* I²C glitches +(the counter resets on every success) before giving up — the chest strip shares +the bus with the IMU and the servo MCU, so occasional NACKs are expected. + +### `_imu_thread` (lines 448–510) + +Two phases. + +**Phase 1 — calibration** (runs once, ~1 s): + +```python +time = 10 # shadows the imported time() function! +for _ in range(time): + data = self.imu._sh3001_getimudata() + ...accumulate... + sleep(0.1) + +self.imu_acc_offset[0] = round(-16384 - _ax/time, 0) +self.imu_acc_offset[1] = round(0 - _ay/time, 0) +self.imu_acc_offset[2] = round(0 - _az/time, 0) +self.imu_gyro_offset[...] = round(0 - _g?/time, 0) +``` + +Averages 10 samples and computes the offsets that would make the readings match +the *expected at-rest values*: **−16384 on X** (i.e. 1 g at ±2 g full scale, +where 16384 LSB = 1 g — so the chip is mounted with its X axis pointing down) +and 0 on the other two accelerometer axes and all three gyro axes. This means +**the robot must be stationary and level for the first second after +construction**, or every later attitude reading is biased. + +`time = 10` shadows the module-level `from time import time` inside this +function; harmless here only because the function never calls `time()`. + +**Phase 2 — the loop** (every 50 ms): + +```python +data = self.imu._sh3001_getimudata() +if data == False: + self.imu_fail_count += 1 + if self.imu_fail_count > 10: + error(...); break +self.accData, self.gyroData = data # <-- executed even when data is False +...apply offsets... +ay = -ay; az = -az +self.pitch = atan(ay / sqrt(ax*ax + az*az)) * 57.2957795 +self.roll = atan(az / sqrt(ax*ax + ay*ay)) * 57.2957795 +``` + +* `57.2957795` is `180/π` — radians to degrees. +* The two `atan(component / magnitude_of_the_other_two)` expressions are the + standard accelerometer tilt estimate. Only the accelerometer is used; the + gyro is read and offset-corrected but never fused (no complementary/Kalman + filter), so `pitch`/`roll` are accurate at rest but noisy while moving. +* The sign flips on `ay`/`az` orient the result to the robot's frame. +* **Bug:** when `data == False` the code increments the counter but does not + `continue`, so it immediately tries `self.accData, self.gyroData = data` and + raises `TypeError: cannot unpack non-sequence bool`. That is caught by the + outer handler, which increments the counter again and sleeps 1 ms — so a + persistent IMU failure becomes a hot loop that reaches the threshold in ~5 + iterations rather than 10, and sets `self.exit_flag = True`, which + **shuts down the leg/head/tail threads too**. An IMU cable fault therefore + bricks all motion. + +--- + +## 8. Buffer control and motion primitives + +### Stopping + +```python +def legs_stop(self): + with self.legs_thread_lock: + self.legs_action_buffer.clear() + self.wait_legs_done() +``` + +Clear the queue, then wait until the worker reports done. `head_stop`, +`tail_stop` are identical; `body_stop()` does all three. Note this does **not** +abort the frame currently being interpolated by `servo_move` — it only discards +what has not started yet. + +### `legs_move(target_angles, immediately=True, speed=50)` + +```python +if immediately: self.legs_stop() # pre-empt whatever is queued +self.legs_speed = speed +with self.legs_thread_lock: + self.legs_action_buffer += target_angles +``` + +`target_angles` is a **list of frames**, each frame an 8-element list in leg-pin +order. `immediately=True` = "interrupt current motion"; `immediately=False` = +"append, play after what's already queued". `ActionFlow`/`do_action` use +`False` for chained animations and `True` for snap-to-pose. + +The speed is stored on the instance, not per frame — so appending frames with a +different speed retroactively changes the speed of frames still in the buffer. + +### `head_rpy_to_angle(target_yrp, roll_comp=0, pitch_comp=0)` (lines 541–548) + +```python +yaw, roll, pitch = target_yrp +signed = -1 if yaw < 0 else 1 +ratio = abs(yaw) / 90 +pitch_servo = roll * ratio + pitch * (1 - ratio) + pitch_comp +roll_servo = -(signed * (roll * (1 - ratio) + pitch * ratio) + roll_comp) +yaw_servo = yaw +``` + +This is the head gimbal's cross-coupling correction. The roll and pitch servos +are mounted **before** the yaw joint in the kinematic chain, so as the head +yaws, the world-frame roll and pitch axes rotate into each other: + +* At `yaw = 0` (`ratio = 0`): `pitch_servo = pitch`, `roll_servo = -(roll)` — + the axes line up directly. +* At `yaw = ±90` (`ratio = 1`): `pitch_servo = roll`, `roll_servo = -(±pitch)` — + the axes have fully swapped. +* In between, the two are linearly blended by `ratio`. + +`signed` handles the sign inversion of the roll servo when the head is turned to +the other side. `roll_comp` / `pitch_comp` are static trims added *after* the +blend — `pitch_comp` is exactly what `ActionFlow` uses to compensate for the +sit/stand body tilt (see `action_flow.md` §8). + +Note `ratio` is not clamped: a yaw beyond ±90 gives `ratio > 1` and starts +extrapolating. The yaw itself is clamped later, in the worker thread, but the +blend uses the unclamped value. + +### `head_move(target_yrps, roll_comp=0, pitch_comp=0, immediately=True, speed=50)` + +Maps `head_rpy_to_angle` over a list of yaw/roll/pitch frames and appends the +resulting servo-angle frames. This is the *normal* head API. + +### `head_move_raw(target_angles, immediately=True, speed=50)` + +Same, bypassing the gimbal maths — the values go straight to the servos (still +clamped and pitch-offset in the worker). Used by `stop_and_lie()` and by +calibration. + +### `tail_move(target_angles, immediately=True, speed=50)` + +Straightforward append. Frames are 1-element lists. + +### `do_action(action_name, step_count=1, speed=50, pitch_comp=0)` (lines 923–938) + +```python +actions, part = self.actions_dict[action_name] +if part == 'legs': + for _ in range(step_count): self.legs_move(actions, immediately=False, speed=speed) +elif part == 'head': + for _ in range(step_count): self.head_move(actions, pitch_comp=pitch_comp, immediately=False, speed=speed) +elif part == 'tail': + for _ in range(step_count): self.tail_move(actions, immediately=False, speed=speed) +``` + +The high-level entry point used everywhere: `do_action('forward', speed=98)`, +`do_action('wag_tail', speed=100)`. Key points: + +* Always `immediately=False` — actions **queue**, they never pre-empt. To + interrupt, callers must call `body_stop()`/`*_stop()` first. +* `step_count` repeats the whole frame set — that is how a multi-step walk is + requested (`do_action('forward', step_count=5)`). +* Names with spaces work because `ActionDict.__getitem__` replaces `' '` with + `'_'`. +* Errors are printed, never raised: `KeyError` → `"do_action: No such action"`, + anything else → `"do_action:"`. Note the lookup actually raises + `AttributeError` (from `eval("self.")`) for an unknown action, so it + lands in the *generic* branch, not the tailored `KeyError` message. + +### Synchronisation helpers (lines 940–967) + +```python +def is_legs_done(self): return not bool(len(self.legs_action_buffer) > 0) +def wait_legs_done(self): + while not self.is_legs_done(): sleep(0.001) +def wait_all_done(self): + self.wait_legs_done(); self.wait_head_done(); self.wait_tail_done() +``` + +1 kHz busy-polling of the buffer lengths. `is_all_done()` is the non-blocking +variant. Reading `len()` without the lock is safe enough in CPython, but these +are the functions that turn the asynchronous buffers into the sequential API +that `preset_actions` and `ActionFlow` rely on. + +Reminder from §7: because head/tail pop *before* moving, `wait_head_done()` +returns slightly early. + +--- + +## 9. Ultrasonic subsystem (lines 576–618) + +```python +def sensory_process_work(self, distance_addr, lock): + echo = Pin('D0'); trig = Pin('D1') + self.ultrasonic = Ultrasonic(trig, echo, timeout=0.017) + self.thread_list.append("ultrasonic") + ... + ultrasonic_thread = threading.Thread(target=self._ultrasonic_thread, + args=(distance_addr, lock,)) + ultrasonic_thread.start() +``` + +* `timeout=0.017` s ≈ 17 ms ≈ **2.9 m** maximum range (sound travels ~343 m/s, + round trip). Beyond that the read times out rather than blocking. +* The device is created **inside the worker**, not in `__init__` — a leftover + from when this was a separate `multiprocessing.Process` and the `Pin` objects + could not be inherited across the fork. +* `sensory_process_work` is itself run in a thread (`sensory_process_start`), + and it spawns *another* thread. Two layers for what is now a single worker. +* Both threads are **non-daemon** (`# ultrasonic_thread.daemon = True` is + commented out), so a process that never calls `close()` will not exit — the + most likely cause of a hung example script. +* `_ultrasonic_thread` writes `distance_addr.value` under `lock` every 10 ms; on + any exception it sleeps 100 ms, prints and **breaks** (distance freezes at its + last value forever). +* The naming (`sensory_process`, `sensory_process_start`) is historical: they + are threads now. `sensory_process_start()` still guards against a previous + instance by setting `_sensory_exit_flag` and `join(timeout=1)` — but it does + not stop the inner `ultrasonic_thread` it spawned, which the flag does cover. + +### `read_distance()` + +```python +return round(self.distance.value, 2) +``` + +Centimetres (as returned by `robot_hat.Ultrasonic.read()`), `-1.0` before the +first successful reading. Negative values are also what the sensor returns on +timeout, so applications treat `distance < 0` as "no echo" — see +`examples/voice_active_dog.py`'s `TOO_CLOSE_DISTANCE` check. + +--- + +## 10. Reset and audio + +### `stop_and_lie(speed=85)` (lines 621–630) + +```python +self.body_stop() +self.legs_move(self.actions_dict['lie'][0], speed) +self.head_move_raw([[0, 0, 0]], speed) +self.tail_move([[0, 0, 0]], speed) +self.wait_all_done() +sleep(0.1) +``` + +Cancels everything queued, then commands the folded lie pose, a centred head and +a centred tail, and blocks until done. Note `tail_move([[0, 0, 0]], ...)` passes +a **3-element** frame to a **1-servo** group; `robot_hat` writes the first value +and ignores the rest, so it works by accident. + +### `speak(name, volume=100)` / `speak_block(name, volume=100)` (lines 632–678) + +Identical except `speak` uses `Music.sound_play_threading` (returns +immediately) and `speak_block` uses `Music.sound_play` (blocks until the clip +ends). Resolution order for `name`: + +1. `name` as a literal path, if `os.path.isfile(name)` +2. `SOUND_DIR + name + '.mp3'` +3. `SOUND_DIR + name + '.wav'` +4. otherwise `warn('No sound found for …')` and return `False` + +Both call `utils.run_command('sudo killall pulseaudio')` on **every invocation** +— a workaround for silent audio under VNC, where a stale PulseAudio daemon owns +the sink. It costs a subprocess per bark and will prompt for a password if the +user has no passwordless sudo. The `is_run_with_root` / `speak_first` dance is +vestigial: it sets a flag and the warning it guarded is commented out. + +Return value is `False` on failure and `None` on success — an inconsistency +callers must not rely on. + +--- + +## 11. Calibration (lines 681–700) + +```python +def set_leg_offsets(self, cali_list, reset_list=None): + self.legs.set_offset(cali_list) + if reset_list is None: + self.legs.reset() + self.leg_current_angles = [0]*8 + else: + self.legs.servo_positions = list.copy(reset_list) + self.legs.leg_current_angles = list.copy(reset_list) + self.legs.servo_write_all(reset_list) + +def set_head_offsets(self, cali_list): + self.head.set_offset(cali_list) + self.head_move([[0]*3], immediately=True, speed=80) + self.head_current_angles = [0]*3 + +def set_tail_offset(self, cali_list): + self.tail.set_offset(cali_list) + self.tail.reset() + self.tail_current_angles = [0] +``` + +`Robot.set_offset()` persists the per-servo trim into `pidog.conf`, so +calibration survives restarts. These are what `bin/`/`pidog-control` calibration +tools drive. + +Two oddities in `set_leg_offsets`: it writes `self.legs.leg_current_angles` +(a new attribute on the `Robot` object) instead of `self.leg_current_angles`, +and it bypasses the buffer/thread by calling `servo_write_all` directly — fine +during calibration, where no animation is running, but it would fight the leg +thread otherwise. `set_head_offsets` correctly goes through `head_move`. + +--- + +## 12. Kinematics + +### 12.1 Frames and conventions + +* **Body frame** — origin at the geometric centre of the four hips, `x` to the + right (`BODY_WIDTH`), `y` forward (`BODY_LENGTH`), `z` up. +* **Leg plane** — each leg is a 2-link planar arm (`LEG = 42`, `FOOT = 76`) + operating in the `(y, z)` plane; there is no abduction joint, which is why the + IK reduces to 2-D. +* Angles are degrees at the public boundary, radians internally + (`self.rpy`). + +### 12.2 `set_pose(x, y, z)` / `set_rpy(roll, pitch, yaw, pid=False)` / `set_legs(legs_list)` + +* `set_pose` — writes the body translation target into `self.pose` (a 3×1 + matrix). Only the components you pass are changed. +* `set_legs(legs_list)` — takes four `[y_offset, z_offset]` pairs and builds + `self.legpoint_struc`, the 3×4 matrix of desired **foot positions**: + `[±W/2, ±L/2 + dy, body_height − dz]`. +* `set_rpy` — two modes: + * **direct** (`pid=False`): `self.rpy = [roll, pitch, yaw] * π/180`. + * **PID** (`pid=True`): a self-levelling step. Error is + `target_rpy − measured` (the measured values come from the IMU thread), the + offset is `KP*e + KI*∫e + KD*Δe`, converted to radians and **added** to the + current `rpy`. So `pid=True` is an *incremental* correction meant to be + called repeatedly in a control loop, whereas `pid=False` is an absolute set. + With `KI = KD = 0` only the proportional term contributes; the integral and + last-error state is still accumulated, ready for tuning. + +### 12.3 `pose2coords()` (lines 757–790) + +Builds the three rotation matrices and composes them: + +```python +rot_mat = rotx * roty * rotz # matrix product (np.matrix semantics) +AB[:, i] = -self.pose - rot_mat * self.BODY_STRUCT[:, i] + self.legpoint_struc[:, i] +``` + +For each leg *i*: take the hip position in the body frame, rotate it by the +commanded body orientation, offset it by the body translation, and subtract it +from the desired foot position. `AB[:, i]` is the hip→foot vector expressed in +the field frame. It returns both lists: + +```python +{"leg": [foot positions (legpoint_struc columns)], + "body": [(legpoint_struc − AB) columns = rotated hip positions]} +``` + +Two things to note about the maths: the matrix named `rotx` is actually a +rotation about **y** (it has the `[cos, 0, -sin; 0, 1, 0; sin, 0, cos]` pattern) +and `roty` is a rotation about **x** — the names are swapped relative to their +content, but since the code consistently feeds `roll` into `rotx` and `pitch` +into `roty`, the resulting behaviour is the intended one for this chassis. Also +`np.matrix` `*` is matrix multiplication, which is exactly why `numpy_mat` is +used instead of plain arrays. + +### 12.4 `pose2legs_angle()` (lines 792–817) + +Reduces each leg's 3-D hip→foot vector to the 2-D `(y, z)` pair the planar IK +needs, solves it, and applies the mirroring convention: + +```python +coords.append([leg_coor[1] - body_coor[1], # Δy + body_coor[2] - leg_coor[2]]) # Δz (sign flipped: down positive) + +leg_angle, foot_angle = self.fieldcoord2polar(coord) +foot_angle = foot_angle - 90 +if i % 2 != 0: # odd index = right side + leg_angle = -leg_angle + foot_angle = -foot_angle +angles += [leg_angle, foot_angle] +``` + +* The `−90` on `foot_angle` re-zeros the knee: the IK returns the interior + angle of the triangle, the servo's zero is the straight-leg position. +* Right-side servos are mirrored (`i % 2 != 0`), because the two sides are + physically mirrored — the same convention `legs_angle_calculation` uses. +* Result is the 8-element list in leg-pin order, ready for `legs_move`. + +### 12.5 `fieldcoord2polar(coord)` / `coord2polar(coord)` (lines 820–856) + +Classic 2-link inverse kinematics via the law of cosines: + +``` +u = √(y² + z²) # hip → foot distance +β = acos((FOOT² + LEG² − u²) / (2·FOOT·LEG)) # knee interior angle +α = atan2(y, z) + acos((LEG² + u² − FOOT²)/(2·LEG·u)) # hip angle +``` + +Both `cos_angle` values are clamped to `[-1, 1]` before `acos`, which is +essential: floating-point error (or a commanded foot position outside the +`|LEG − FOOT| … LEG + FOOT` reachable annulus) would otherwise raise +`ValueError: math domain error`. Clamping silently saturates at the workspace +boundary instead. + +The **only** difference between the two functions: + +```python +# fieldcoord2polar +alpha = angle2 + angle1 + self.rpy[1] # + commanded body pitch +# coord2polar +alpha = angle2 + angle1 +``` + +`fieldcoord2polar` works in the *field* frame — the coordinates came out of +`pose2coords`, which already applied the body rotation, so the hip angle must be +corrected back by the body pitch. `coord2polar` works in the *robot* frame and +needs no such term. The duplication (rather than one function with a flag) is +the main refactoring opportunity in this section. + +### 12.6 `polar2coord(angles)` (lines 858–870) + +The nominal forward-kinematics inverse of the above, taking `[alpha, beta, +gamma]`. **It is broken**: it references `self.A`, `self.B`, `self.C`, none of +which exist on `Pidog`. Calling it raises `AttributeError`. Its only call site +is the error branch of `set_angle()`, which is itself unreachable (§12.8). + +### 12.7 `legs_angle_calculation(coords)` (lines 872–886) + +```python +@classmethod +def legs_angle_calculation(cls, coords): + for i, coord in enumerate(coords): + leg_angle, foot_angle = Pidog.coord2polar(cls, coord) + foot_angle = foot_angle - 90 + if i % 2 != 0: + leg_angle, foot_angle = -leg_angle, -foot_angle + translate_list += [leg_angle, foot_angle] +``` + +The **class-level** IK entry point: four `[y, z]` foot coordinates in → +eight joint angles out. This is what `actions_dictionary.py`, `walk.py` and +`trot.py` call to precompute gait frames **without instantiating a `Pidog`** — +which matters because `ActionDict` is built during `Pidog.__init__` itself. + +The trick `Pidog.coord2polar(cls, coord)` calls the *unbound* instance method +with the **class object** standing in for `self`. That works only because +`coord2polar` touches nothing but class constants (`self.FOOT`, `self.LEG`) — +attribute lookup on the class finds them. It would break the moment +`coord2polar` referenced instance state, which is precisely why +`fieldcoord2polar` (which reads `self.rpy`) cannot be used this way. + +### 12.8 `limit()` and `set_angle()` (lines 889–920) + +`limit(min, max, x)` is a plain clamp (shadowing the builtins `min`/`max` as +parameter names). Used by `_head_action_thread`. + +`set_angle(angles_list, speed=50, israise=False)` is **dead code**: it calls +`self.limit_angle()`, `self.polar2coord()`, `self.coord_temp` and +`self.servo_move()` — of those, only `polar2coord` exists (and is itself +broken). Nothing in the repository calls `set_angle`. It is a remnant of an +older API and should be deleted. + +--- + +## 13. Miscellaneous + +* **`legs_simple_move(angles_list, speed=90)`** (lines 329–353) — bypasses the + buffer/thread entirely and writes raw servo values with + `self.legs.servo_write_raw(angles + offset)`, then sleeps a speed-derived + delay (0.005 s at speed 100 → 0.05 s at speed 0), reduced by the time the + write itself took. Used for tight custom loops (e.g. the calibration and + balance demos) where the interpolation in `servo_move` gets in the way. + Because it applies `self.legs.offset[i]` manually and writes raw, it does + **not** respect `max_dps`. +* **`legs_switch(flag)`** (lines 355–356) — sets `self.legs_sw_flag`, which + nothing reads. Dead. +* **`get_battery_voltage()`** — `round(utils.get_battery_voltage(), 2)`, volts. + A 2S 18650 pack: ~8.4 V full, ~6.6 V empty. +* **`self.touch`** is initialised to `'N'` but never updated by `Pidog`; + applications poll `self.dual_touch.read()` themselves (returning + `TouchStyle` values `'N' | 'L' | 'R' | 'LS' | 'RS'`). +* Likewise `self.ears` (sound direction) is created but never polled here — + `examples/voice_active_dog.py` calls `ears.isdetected()` / `ears.read()`. + +--- + +## 14. Known issues / improvement candidates + +Ordered roughly by impact. + +1. **`_imu_thread` does not `continue` after `data == False`** — it unpacks a + bool, raises `TypeError`, and on repeated failure sets `self.exit_flag = True`, + which stops the leg, head and tail threads. An IMU fault therefore disables + all motion. +2. **`compare_version` returns `or_v >= or_v`** — always `True`; the version + check never actually runs. The dead `else` branch defines `numpy_mat` as + infinitely recursive (`return numpy_mat(data)` instead of `np.mat(data)`). +3. **`leg_point_struc` vs `legpoint_struc`** — `__init__` sets the former, + `set_legs()`/`pose2coords()` use the latter. `pose2coords()` before + `set_legs()` raises `AttributeError`. +4. **`polar2coord` references non-existent `self.A/B/C`** — always raises. +5. **`set_angle` is dead code** referencing three more non-existent members + (`limit_angle`, `coord_temp`, `servo_move`). +6. **`close()` never cancels the 5 s `SIGALRM` nor restores the SIGINT + handler** (the `finally:` block is commented out), so a `TimeoutError` can + fire later at an arbitrary point. +7. **Ultrasonic threads are non-daemon**, so forgetting `close()` hangs process + exit. +8. **Head/tail pop before moving, legs pop after** — `wait_head_done()` / + `wait_tail_done()` return before the motion physically completes. +9. **Speed is per-group state, not per-frame** — a later `*_move()` with a + different speed retroactively changes frames already queued. +10. **`speak()` shells out to `sudo killall pulseaudio` on every call** — a + subprocess per sound effect, and it fails noisily without passwordless sudo. +11. **Worker threads `break` on any unexpected exception**, permanently and + silently disabling that body part; there is no restart or health check. +12. **Idle worker threads spin at 1 kHz raising `IndexError`**; a + `threading.Condition` or `queue.Queue` would be both cheaper and race-free. +13. **`do_action`'s `except KeyError` branch is unreachable** — `ActionDict.__getitem__` + uses `eval("self.")`, so an unknown action raises `AttributeError` + and gets the generic message. +14. `head_init_angles` is **mutated in place** when supplied by the caller. +15. Unused imports/attributes: `Process`, `I2C`, `sin`, `legs_actions_coords_buffer`, + `legs_switch`/`legs_sw_flag`, `self.touch`. +16. Inconsistent error handling: `except OSError` for some peripherals, bare + `except:` for others (which also swallows `KeyboardInterrupt`). +17. `stop_and_lie()` passes a 3-element frame to the 1-servo tail group. +18. `== None` comparisons instead of `is None` throughout. diff --git a/docs/preset_actions.md b/docs/preset_actions.md new file mode 100644 index 0000000..3626629 --- /dev/null +++ b/docs/preset_actions.md @@ -0,0 +1,657 @@ +# `pidog/preset_actions.py` — Detailed Documentation + +This document explains `pidog/preset_actions.py` in depth: the conventions every +routine follows, what each of the 24 functions does frame by frame, the shared +angle vocabulary, and the sharp edges / latent bugs in the current +implementation. + +Companion documents: [`pidog.md`](./pidog.md) (the hardware driver these +routines call) and [`action_flow.md`](./action_flow.md) (the scheduler that +invokes them by name). + +--- + +## 1. Purpose and place in the codebase + +`preset_actions.py` is the **choreography library**: a flat collection of +module-level functions, each of which plays one hand-authored, multi-step +animation on a `Pidog` instance. + +``` +examples/*.py <- import individual routines directly + | +pidog/action_flow.py (ActionFlow) <- maps names like "high five" -> these functions + | + v +pidog/preset_actions.py <<< THIS FILE >>> + | + v +pidog/pidog.py (Pidog) <- legs_move / head_move / do_action / speak + | + v +pidog/actions_dictionary.py <- named single poses ('sit', 'stand', 'push_up', …) +``` + +The division of labour is worth stating precisely: + +| Layer | Owns | +| --- | --- | +| `actions_dictionary.py` | **Static poses and generated gaits** — a name maps to a list of joint-angle frames plus the body part. No sequencing, no sound, no timing. | +| `preset_actions.py` | **Sequences** — several poses in order, mixing legs + head + tail, interleaved with sounds, `sleep()`s, randomness and repetition. | +| `action_flow.py` | **Behaviour selection** — which sequence to play, posture preconditions, queueing. | + +Everything here is a plain function taking the `Pidog` object as its first +argument (conventionally named `my_dog`). There is no class, no state (with one +vestigial exception, §4.4), and no return values — these are pure side-effect +routines. + +--- + +## 2. Imports and module surface (lines 1–4) + +```python +from time import sleep +import random +from math import sin, cos, pi +``` + +* `sleep` — inter-phase pauses that cannot be expressed as servo motion (letting + a sound finish, holding a pose). +* `random` — used by `waiting()` and `feet_shake()` to make idle behaviour + non-repetitive. +* `sin`, `cos`, `pi` — used by the four *procedural* animations + (`shake_head_smooth`, `relax_neck`, `nod`) that generate smooth frame + sequences instead of listing poses by hand. + +There is **no `__all__`**, which matters: `action_flow.py` does +`from .preset_actions import *`, so it also inherits `sleep`, `random`, `sin`, +`cos` and `pi` into its namespace — and it *depends* on that, because +`ActionFlow.action_handler` calls `random.choices()` / `random.randint()` +without importing `random` itself. Adding an `__all__` here would break +`action_flow.py` unless that module gains its own `import random`. + +--- + +## 3. Shared conventions + +Understanding these five conventions makes every function in the file readable. + +### 3.1 Leg angle frames — 8 values + +Every leg frame is an 8-element list in **leg-pin order**, matching +`Pidog.DEFAULT_LEGS_PINS = [2, 3, 7, 8, 0, 1, 10, 11]`: + +``` +index: 0 1 2 3 4 5 6 7 + LF hip LF knee RF hip RF knee LH hip LH knee RH hip RH knee + \___ left front ___/ \__ right front __/ \__ left hind __/ \__ right hind __/ +``` + +Left and right are **mirrored**, so a symmetric pose has `[a, b, -a, -b, c, d, -c, -d]`. +The canonical sit pose from `actions_dictionary.py` is exactly that: + +```python +sit = [30, 60, -30, -60, 80, -45, -80, 45] +``` + +Almost every frame in this file is a small perturbation of `sit`, which is why +the constant `80, -45, -80, 45` tail-half appears over and over: **the hind legs +stay in the sit pose while the front legs perform the trick**. + +Note 1 in the source (`# Note 1`, lines 11–16, 28) documents one such tweak: + +> Last servo (4th legs) original value is 45, change to 40 to push down a little +> bit to support the raising legs, prevent the dog from falling down. + +i.e. index 7 is dropped from `45` → `38` in `scratch`, `hand_shake` and +`high_five` so the right hind leg braces against the tipping moment created by +lifting a front paw. + +### 3.2 Head frames — 3 values + +Head frames are `[yaw, roll, pitch]` in degrees, and there are **two different +APIs**, used deliberately: + +| Call | Meaning | +| --- | --- | +| `my_dog.head_move(frames, pitch_comp=…)` | Goes through `Pidog.head_rpy_to_angle`, which blends roll/pitch as a function of yaw to compensate for the gimbal's cross-coupling. Use when the head is turned. | +| `my_dog.head_move_raw(frames)` | Bypasses that blend; the values go straight to the servos (still clamped and pitch-offset in the worker thread). Use for precomputed servo-space sequences. | + +The procedural animations (`shake_head_smooth`, `relax_neck`, `nod`, `think`, +`recall`, `fluster`, `alert`, `surprise`) all use `head_move_raw` and fold +`pitch_comp` into the pitch value themselves (`p = … + pitch_comp`), because +they were tuned in servo space. The pose-based ones (`pant`, `bark`, +`bark_action`, `shake_head`) use `head_move` and pass `pitch_comp=` through. + +`pitch_comp` is the posture compensation supplied by `ActionFlow`: `0` when +standing, `-35` when sitting (see `action_flow.md` §5). + +### 3.3 `immediately=False` and the wait dance + +Recall from [`pidog.md`](./pidog.md) that `legs_move`/`head_move` are +**non-blocking** — they append frames to per-group buffers that background +threads drain. Therefore: + +```python +my_dog.legs_move(f_up, immediately=False, speed=80) # queue, do not pre-empt +my_dog.wait_all_done() # block until buffers empty +``` + +* `immediately=False` = *append* (chain onto whatever is queued). + `immediately=True` = *pre-empt* (clear the buffer first). This file uses + `False` almost everywhere; `bark_action` and `attack_posture` use `True` + because a bark must interrupt whatever the dog was doing. +* Issuing legs **and** head before a single `wait_all_done()` is how the two are + made to move *simultaneously*; issuing them with a wait in between makes them + sequential. Read every routine with that in mind — the placement of the wait + calls *is* the choreography. +* `wait_legs_done()` / `wait_head_done()` allow waiting on one group only, e.g. + `lick_hand` waits on both explicitly rather than using `wait_all_done()` + (equivalent, since the tail is idle). + +### 3.4 Speed + +`speed` is 0–100 and is **per group, not per frame** (see `pidog.md` §8): +the last value passed wins for everything currently in that buffer. Typical +values in this file: 50–68 for deliberate/slow motions, 80 for normal, 90–100 +for snappy ones (scratching, high-fiving, barking, flustering). + +### 3.5 Sound + +`my_dog.speak(name, volume)` is **non-blocking** and resolves `name` against +`~/pidog/sounds/{name}.mp3|.wav`. Available clips in the repo's `sounds/`: +`angry.wav`, `confused_1..3.mp3`, `growl_1..2.mp3`, `howling.mp3`, `pant.mp3`, +`single_bark_1.mp3`, `single_bark_2.mp3`, `snoring.mp3`, `woohoo.mp3`. + +Because playback is asynchronous, routines that need audio and motion to line up +either start the sound *first* and then move (`pant`, `bark`) or insert an +explicit `sleep()` sized to the clip (`howling`, §4.7). + +--- + +## 4. The routines + +24 functions, grouped by kind. + +### 4.1 Front-paw tricks (sit-based) + +These four share a skeleton: **raise a front paw → repeat a small oscillation → +withdraw → settle back into sit with the head lowered**. They all assume the dog +is (or will be put) sitting, and all keep the hind legs at the sit angles. + +#### `scratch(my_dog)` (lines 7–28) + +```python +h1 = [[0, 0, -40]] # head level-ish, looking down +h2 = [[30, 70, -10]] # head turned+rolled toward the scratching paw +f_up = [[30, 60, 50, 50, 80, -45, -80, 38]] +f_scratch = [[30, 60, 40, 40, 80, -45, -80, 38], + [30, 60, 50, 50, 80, -45, -80, 38]] +``` + +1. `do_action('sit', speed=80)` — establish the base pose. +2. Head to `h2` and front-right paw up (`f_up`) **together**, then wait. The + large roll (`70`) tilts the head toward the raised paw — this is what sells + the "scratching my ear" read. +3. **10 iterations** of the two-frame `f_scratch` oscillation at `speed=94` + (knee 50↔40 = a 10° flutter). Waiting inside the loop means each pair + completes before the next is queued. +4. Head back to `h1`, re-issue `sit`, wait. + +Note index 7 is `38` throughout (Note 1 bracing) but the final `sit` restores it +to `45`. + +#### `hand_shake(my_dog)` (lines 31–62) + +```python +f_up = [[30, 60, -20, 65, 80, -45, -80, 38]] # paw offered +f_handshake = [[30, 60, 10, -25, 80, -45, -80, 38], + [30, 60, 10, -35, 80, -45, -80, 38]] # 10° pump +f_withdraw = [[30, 60, -40, 30, 80, -45, -80, 38]] +``` + +Offer the paw, `sleep(0.1)` (a beat, so a human can grab it), pump **8 times** +at `speed=90`, withdraw, then a **4-frame** descent +(`-40 → -50 → -58 → -60` on the right-front knee) which lands the paw gently +instead of dropping it, issued together with `head_move([[0, 0, -35]])`. + +That 4-frame `hand_down_angs` block is copy-pasted verbatim into `high_five` and +`lick_hand` — the obvious extraction candidate in this file. + +#### `high_five(my_dog)` (lines 65–93) + +Same skeleton with three poses instead of a loop: paw up (`f_up`), a fast +slap down (`f_down`, `speed=94`), `sleep(0.5)` to hold the contact, withdraw, +then the same 4-frame gentle descent. The `speed=94` on the down stroke versus +`80` elsewhere is the whole gag. + +#### `lick_hand(my_dog)` (lines 234–271) + +```python +leg1 = [[30, 45, 70, -32, 80, -55, -80, 45]] +head1 = [[-22, -23, -45], + [-22, -23, -35]] # 10° pitch bob = the "lick" +leg2 = [[30, 45, 70, -32, 80, -55, -80, 45], + [30, 45, 66, -36, 80, -55, -80, 45]] +``` + +Sit, head down (`immediately=True` — pre-empt whatever the head was doing), then +raise the paw toward the muzzle (`leg1`) while the head bobs (`head1`), then +**3 repetitions** of paw-and-head bobbing together, then the shared 4-frame +descent. The negative yaw/roll on the head (`-22, -23`) tilts it toward the +raised paw, mirroring the trick in `scratch`. + +Note this routine waits with `wait_head_done()` + `wait_legs_done()` rather than +`wait_all_done()`; functionally the same here. + +### 4.2 Full-body motions + +#### `body_twisting(my_dog)` (lines 109–124) + +```python +f1 = [-80, 70, 80, -70, -20, 64, 20, -64] # the stretch pose (== actions_dict['stretch']) +f2 = [-70, 50, 80, -90, 10, 20, 20, -64] # twisted left +f3 = [-80, 90, 70, -50, -20, 64, -10, -20] # twisted right +f = [f2, f1, f3, f1] # left → centre → right → centre +``` + +A slow (`speed=50`) four-frame twist about the long axis from the sprawled +stretch pose, then `sleep(.3)` and a **two-stage** return to sit +(`_2_sit_angs`: an intermediate half-crouch, then the sit pose) at `speed=68`, +with the head raised to `-35` via `head_move_raw`. The intermediate frame +matters — going straight from the sprawl to sit makes the legs scrape. + +#### `stretch(my_dog)` (lines 529–552) + +The classic dog stretch ("play bow"): five leg frames alternating the front-hip +angle `-80 → -80 → -65 → -80 → -65` at `speed=55`, i.e. a couple of slow +pushes deeper into the stretch, with the head lifted to pitch `25`. Then the +same `sleep(.3)` + two-stage `_2_sit_angs` return as `body_twisting` (the two +functions share that ending verbatim). + +#### `push_up(my_dog, speed=80)` (lines 195–198) + +```python +my_dog.head_move([[0, 0, -80], [0, 0, -40]], speed=speed-10) +my_dog.do_action('push_up', speed=speed) +my_dog.wait_all_done() +``` + +Delegates the leg work to `actions_dict['push_up']` (two frames) and adds a head +dip/lift, deliberately **10 slower** than the legs so the head lags — the head +"follows" the body rather than snapping with it. Note `-80` pitch is far outside +`HEAD_PITCH_MIN = -45` and gets clamped to `-45` in the worker thread; the frame +is effectively "head fully down". + +#### `sit_2_stand(my_dog, speed=75)` (lines 326–340) + +```python +sit_angles = my_dog.actions_dict['sit'][0][0] # fetched, unused +stand_angles = my_dog.actions_dict['stand'][0][0] +L1 = [25, 25, -25, -25, 70, -25, -70, 25] # brace pose +my_dog.legs_move([L1, stand_angles], immediately=False, speed=speed) +``` + +Standing up from a sit cannot be a single interpolation — the feet would slip +and the dog would fall backwards. `L1` is an intermediate pose that tucks the +front legs under the body first; only then does it extend to `stand_angles`. +`ActionFlow.change_poseture` calls this with `speed=75` and the comment +`# speed > 70`, because below ~70 the servos are too slow to overcome the body +weight mid-transition. + +`sit_angles` is read and never used — the commented-out first frame on line 334 +shows it used to be the sequence's starting frame. + +Note `stand_angles` is *computed*, not a literal: `actions_dictionary.stand` +calls `Pidog.legs_angle_calculation(...)` with the current barycentre and +height, so this routine automatically respects +`ActionDict.set_height()` / `set_barycenter()`. + +#### `feet_shake(my_dog, step=None)` (lines 285–323) + +The only routine that builds its frames **relative to the current pose**: + +```python +current_legs = list.copy(my_dog.leg_current_angles) +L1 = list.copy(current_legs); L1[0] += 10; L1[1] -= 25 # left front shakes +L2 = list.copy(current_legs); L2[2] -= 10; L2[3] += 25 # right front shakes +``` + +Then it randomly picks one of three sequences — `[L1, L1, L2, L2]` (both paws), +`[L1, current]` (left only), `[L2, current]` (right only) — repeats it +`step` times (`random.randint(1, 2)` if not given) at a lazy `speed=45`, and +finally returns to `sit` with the head at `-40`. + +Two consequences of the relative construction: it only looks right if +`leg_current_angles` is a sit-like pose when called, and `leg_current_angles` is +the *commanded* angle from the driver's worker thread, so calling this mid-motion +snapshots an intermediate pose. + +### 4.3 Bark / alert family + +#### `bark(my_dog, yrp=None, pitch_comp=0, roll_comp=0, volume=100)` (lines 178–192) + +Head-only bark: + +```python +head_up = [0+yrp[0], 0+yrp[1], 25+yrp[2]] +head_down = [0+yrp[0], 0+yrp[1], 0+yrp[2]] +my_dog.wait_head_done() # let any prior head motion settle +head_move([head_up], …, immediately=True, speed=100) +my_dog.speak('single_bark_1', volume) # fire the sound as the head snaps up +my_dog.wait_head_done(); sleep(0.08) +head_move([head_down], …, immediately=True, speed=100) +my_dog.wait_head_done(); sleep(0.5) # refractory gap between barks +``` + +`yrp` is a base orientation the whole gesture is offset by — `ActionFlow` passes +`self.head_yrp` so the bark happens *wherever the head is currently aimed* +(e.g. at a tracked face) rather than always straight ahead. The `sleep(0.08)` +is the snap-and-hold at the top; the trailing `sleep(0.5)` prevents machine-gun +barking when called in a loop. + +Note the hard-coded sound: unlike `bark_action`, `bark` always plays +`single_bark_1`. + +#### `bark_action(my_dog, yrp=None, speak=None, volume=100)` (lines 127–147) + +Whole-body bark — the front end lunges as the head snaps up: + +```python +f1 = my_dog.legs_angle_calculation([[0, 100], [0, 100], [30, 90], [30, 90]]) +f2 = my_dog.legs_angle_calculation([[-20, 90], [-20, 90], [0, 90], [0, 90]]) +``` + +These are the only frames in the file expressed as **foot coordinates** +(`[y, z]` in mm per leg) and run through the inverse kinematics +(`Pidog.legs_angle_calculation`, a classmethod — see `pidog.md` §12.7) instead of +being hand-tuned joint angles. `f1` = tall and rocked back (z = 100 front, +90 hind); `f2` = front feet pulled 20 mm backwards and lowered = the lunge. + +Legs and head are moved together with `immediately=True` (pre-empt), the sound +is optional (`speak=None` ⇒ silent lunge). `ActionFlow` maps `"bark harder"` to +`bark_action(..., 'single_bark_1')` preceded by `attack_posture`. + +#### `attack_posture(my_dog)` (lines 225–231) + +Just the `f2` pose from `bark_action` (the crouched lunge stance), held. Used as +the `before` step of `"bark harder"` so the dog coils before it barks. + +#### `alert(my_dog, pitch_comp=0)` (lines 465–486) + +Two-frame body startle (legs stiffen: hind hips `80 → 88`; head dips 5° then +lifts 10°) at `speed=100`, then a slow scan: yaw `+30`, `sleep(1)`, yaw `-30`, +`sleep(1)`, back to centre. The one-second holds are what make it read as +"looking around for the source" rather than a twitch. + +Not referenced by `ActionFlow`; available for direct use by examples. + +### 4.4 Idle / breathing + +#### `waiting(my_dog, pitch_comp)` (lines 273–283) + +```python +global last_wait # vestigial: declared, never assigned or read +p0..p3 = [0, ±7, pitch_comp±5] +choice = random.choices(p, [1,1,1,1])[0] +my_dog.head_move([choice], immediately=False, speed=5) +my_dog.wait_head_done() +``` + +Picks one of four tiny head offsets (±7° roll, ±5° pitch around the compensated +neutral) and drifts to it at **`speed=5`** — the slowest speed used anywhere in +the package. This is the "idling / breathing" motion `ActionFlow` plays every +2–6 seconds while in `STANDBY`. Uniform weights make `random.choices` here +equivalent to `random.choice`. + +The `global last_wait` statement is dead: nothing in the package defines or +assigns `last_wait`. It presumably once prevented picking the same offset twice +in a row. + +`pitch_comp` is **positional and required** here, unlike every other routine +where it is a keyword with a default. + +### 4.5 Procedural head animations (sin/cos generated) + +These four build a dense list of frames from a trigonometric expression and hand +the whole list to `head_move_raw` in one call, so the driver interpolates +through them back-to-back — that is what makes them look smooth rather than +stepped. + +#### `shake_head_smooth(my_dog, pitch_comp=0, amplitude=40, speed=90)` (lines 162–175) + +```python +for i in range(0, 31, 2): + y = round(amplitude * sin(pi/10 * i), 2) + angs.append([y, 0, pitch_comp]) +``` + +`sin(π·i/10)` has period `i = 20`, so `i` from 0 to 30 in steps of 2 gives +**1.5 full cycles** in 16 frames: right, left, right, ending mid-swing at +`sin(3π) = 0` — i.e. back at centre. Pure yaw; roll fixed at 0 and pitch fixed +at the compensation value. + +#### `shake_head(my_dog, yrp=None)` (lines 150–159) + +The non-procedural version: three discrete poses (`+40`, `-40`, `0` yaw) at +`speed=92`, queued back-to-back. Default `yrp = [0, 0, -20]`, i.e. a slightly +lowered head — note this default differs from every other routine's `[0, 0, 0]`. +`ActionFlow` calls this one, adding `head_pitch_init` into the pitch element +manually because `shake_head` has no `pitch_comp` parameter. + +#### `nod(my_dog, pitch_comp=-35, amplitude=20, step=2, speed=90)` (lines 387–400) + +```python +for i in range(0, 20*step+1, 2): + p = round(amplitude * cos(pi/10 * i) - amplitude + pitch_comp, 2) + angs.append([0, 0, p]) +``` + +`cos(π·i/10) − amplitude` maps the cosine into `[-2·amplitude, 0]`, so the head +**starts at neutral and only ever dips downwards** — a nod, not an oscillation +about centre. `step` is the number of nods (`20·step` covers `step` full +cosine periods). + +#### `relax_neck(my_dog, pitch_comp=-35)` (lines 342–384) + +Two phases. + +*Phase 1 — a rolling neck circle*, 21 procedurally generated frames: + +```python +y_ang = round(10 * sin(pi/10*i), 2) +r_ang = round(45 * sin(pi/10*i), 2) +p_ang = round(20 * sin(pi/10*i - pi/2) + pitch_comp, 2) +``` + +Yaw and roll are in phase (amplitudes 10 and 45), pitch is **90° out of phase** +(`−π/2`) with amplitude 20 — a phase offset between orthogonal axes is exactly +what traces a circle, so the muzzle sweeps a cone. `i` from 0 to 20 = one full +period. + +*Phase 2 — discrete side stretches*: roll to `+45`, back off to `+25`, again, +return to centre, hold two frames, then the mirror image on `-45`/`-25`. The +duplicated `[0, 0, 5+pitch_comp]` frames are the hold at centre; the trailing +frame drops the `+5` to end at exactly `pitch_comp`. Commented-out `±35` frames +show the original, gentler tuning. + +### 4.6 Expressive head poses (single frame) + +Four near-identical one-liners; each queues a single `head_move_raw` frame at +`speed=80` and waits: + +| Function | Frame `[yaw, roll, pitch]` | Reads as | +| --- | --- | --- | +| `think(pitch_comp=0)` | `[20, -15, 15+pitch_comp]` | head cocked up-and-left | +| `recall(pitch_comp=0)` | `[-20, 15, 15+pitch_comp]` | the mirror image — looking up-and-right | +| `head_down_left(pitch_comp=0)` | `[25, 0, -35+pitch_comp]` | looking down-left | +| `head_down_right(pitch_comp=0)` | `[-25, 0, -35+pitch_comp]` | looking down-right | + +`think`/`recall` are wired into `ActionFlow`; the two `head_down_*` are not, and +have no callers in the repo. + +#### `fluster(my_dog, pitch_comp=0)` (lines 437–463) + +Five fast repetitions (`speed=100`) of a four-frame yaw jitter +(`-10 → 0 → +10 → 0`). The leg half of the routine is **fully commented out**: + +```python +# current_legs = list.copy(my_dog.leg_current_angles) +current_legs = [30, 60, -30, -60, 80, -45, -80, 45] +L1 = …; L2 = …; leg1 = [L1, L1, L2, L2] +for _ in range(5): + # my_dog.legs_move(leg1, immediately=False, speed=100) + my_dog.head_move_raw(h_l, speed=100) +``` + +So `current_legs`, `L1`, `L2` and `leg1` are all computed and discarded every +call — dead code. Note also `L2[3]` is never adjusted here, unlike the +equivalent block in `feet_shake`, which suggests the leg half was abandoned +mid-tuning. + +#### `surprise(my_dog, pitch_comp=0, status='sit')` (lines 489–526) + +The only routine with a posture parameter, because a startle looks different +sitting versus standing: + +* `status='sit'` — front knees snap `50 → 80`, hind hips `80 → 88` (the body + rears back), head dips 5° then lifts 10°, all at `speed=100`; `sleep(1)` to + hold the reaction; then a `speed=80` relax back to the plain sit pose. +* `status='stand'` — a smaller version from the stand pose, at `speed=80` + throughout (a standing dog cannot snap as hard without falling). + +`ActionFlow` always calls it with the default `'sit'`. + +### 4.7 Sound-led routines + +#### `pant(my_dog, yrp=None, pitch_comp=0, speed=80, volume=100)` (lines 96–107) + +```python +h = [h1, h2, h1] # neutral, -10° pitch, neutral +my_dog.speak('pant', volume) # start the audio first (non-blocking) +sleep(0.01) +for _ in range(6): + my_dog.head_move(h, pitch_comp=pitch_comp, immediately=False, speed=speed) + my_dog.wait_head_done() +``` + +Six head bobs synchronised *by construction* to `pant.mp3` — the sound is +started first and the 6 × 3-frame motion is tuned to run for about as long. The +`sleep(0.01)` yields long enough for the audio thread to actually begin. + +#### `howling(my_dog, volume=100)` (lines 201–222) + +The most elaborate sequence, and the only one that drives the **RGB chest +strip**: + +1. Sit, head to pitch `-30` (`speed=95`). +2. `rgb_strip.set_mode('speak', color='cyan', bps=0.6)` — the chest light + pulses in "speak" style at 0.6 beats/s for the duration of the howl. +3. `half_sit` (a crouch, from `actions_dictionary`) with the head dropped to + `-60` — winding up. +4. `speak('howling', volume)` and simultaneously rise back to `sit` with the head + thrown up to `+10` — the howl itself. +5. The same sit + head-up pose is issued **a second time** (lines 215–217) at a + different head speed; since the pose is already reached this is effectively a + no-op hold. +6. `sleep(2.34)` — a magic number: the remaining length of `howling.mp3`, held + with the head up so the pose lasts as long as the audio. +7. Return to sit with the head down at `-40`. + +The RGB mode is **never reset** — the chest strip stays in cyan "speak" mode +after the routine ends; the caller has to set it back. Note also that `-60` +pitch is clamped to `HEAD_PITCH_MIN = -45` by the driver. + +--- + +## 5. The `__main__` block (lines 554–655) + +```python +if __name__ == "__main__": + from pidog import Pidog + import readchar + yrp = [0, 0, -40] + my_dog = Pidog() + my_dog.rgb_strip.set_mode('listen', 'cyan', 1) + my_dog.do_action('sit', speed=80) + my_dog.head_move_raw([[0, 0, -25]], immediately=False, speed=68) + my_dog.wait_all_done() + sleep(.5) + # scratch(my_dog) + # while True: nod(my_dog, …) + … +``` + +A **developer scratchpad**, not a demo. Running +`python3 -m pidog.preset_actions` initialises the robot, sets the chest strip to +cyan "listen", sits it down with the head at `-25`, waits half a second — and +then does nothing, because every actual test invocation below is commented out. +The pattern (`while True: ; sleep(2)`) is how each animation was tuned: +uncomment one block, run, adjust the constants, repeat. + +Consequences: `readchar` is imported but unused (an extra dependency for anyone +running the module directly), `yrp` is assigned and unused, `my_dog.close()` is +commented out so the process never exits cleanly (the ultrasonic threads are +non-daemon — see `pidog.md` §9), and the file has no `-h`/argument handling. + +--- + +## 6. Cross-reference: who calls what + +| Routine | `ActionFlow` name | Direct example users | +| --- | --- | --- | +| `bark` | `bark` | `3_patrol.py`, `7_face_track.py`, `8_pushup.py`, `13_ball_track.py` | +| `bark_action` | `bark harder` | `4_response.py` | +| `attack_posture` | `bark harder` (as `before`) | — | +| `pant` | `pant` | `1_wake_up.py` | +| `body_twisting` | `twist body` | `1_wake_up.py` | +| `shake_head` | `shake head` | `5_rest.py` | +| `push_up` | `push up` | `8_pushup.py` | +| `howling` | `howling` | `9_howling.py` | +| `stretch` | `stretch` | — | +| `scratch`, `hand_shake`, `high_five`, `lick_hand`, `feet_shake`, `waiting`, `relax_neck`, `nod`, `think`, `recall`, `fluster`, `surprise` | same-named entries | via `11_keyboard_control.py` / `12_app_control.py` star-imports | +| `sit_2_stand` | used by `change_poseture`, not an action | — | +| `shake_head_smooth`, `alert`, `head_down_left`, `head_down_right` | **not wired in** | only the commented `__main__` blocks | + +--- + +## 7. Known issues / improvement candidates + +1. **No `__all__`** — the star-import in `action_flow.py` silently re-exports + `random`, `sleep`, `sin`, `cos`, `pi`, and `ActionFlow` *depends* on the + `random` leak. Adding `__all__` without adding `import random` to + `action_flow.py` breaks the idle loop. +2. **`fluster` computes `current_legs`, `L1`, `L2`, `leg1` and discards them** — + the leg half is commented out. Either restore it or delete the dead code. +3. **`global last_wait` in `waiting()`** refers to a variable that does not + exist anywhere in the package. +4. **`sit_2_stand` fetches `sit_angles` and never uses it.** +5. **The 4-frame `hand_down_angs` block is duplicated verbatim three times** + (`hand_shake`, `high_five`, `lick_hand`), and the `_2_sit_angs` ending twice + (`body_twisting`, `stretch`). Both are extraction candidates. +6. **Magic sleeps tied to audio length** — `sleep(2.34)` in `howling`, the + 6-iteration loop in `pant`. Any change to the sound files silently desyncs + the animation; there is no query of the clip duration. +7. **`howling` leaves the RGB strip in `'speak'`/cyan mode** and never restores + the previous mode. +8. **Out-of-range head angles are relied upon to be clamped** by the driver: + `push_up` commands pitch `-80` and `howling` commands `-60`, both clamped to + `HEAD_PITCH_MIN = -45`. Works, but the intent ("as far down as possible") + is not explicit. +9. **`waiting(my_dog, pitch_comp)` takes `pitch_comp` positionally with no + default**, inconsistent with every other routine. +10. **`shake_head` defaults to `yrp=[0, 0, -20]`** while all its siblings default + to `[0, 0, 0]`, and it has no `pitch_comp` parameter — callers must fold the + compensation into the pitch element themselves. +11. **`feet_shake` reads `my_dog.leg_current_angles`**, the *commanded* pose from + the driver's worker thread; calling it mid-motion produces a distorted + animation. It also assumes a sit-like starting pose. +12. **No error handling anywhere** — a missing sound file only produces a warning + from `Pidog.speak`, but a hardware fault mid-routine propagates to the + caller (`ActionFlow.run` catches and prints it). +13. **`__main__` imports `readchar` without using it** and never calls + `my_dog.close()`, so running the module directly leaves the process hanging. +14. **Repetition counts are hard-coded** (`range(10)` in `scratch`, `range(8)` in + `hand_shake`, `range(3)` in `lick_hand`, `range(5)` in `fluster`, + `range(6)` in `pant`) — unlike `nod`/`feet_shake`, which take a `step` + parameter. diff --git a/examples/19_voice_active_dog_ollama.py b/examples/19_voice_active_dog_ollama.py index e79c21e..8af6a20 100644 --- a/examples/19_voice_active_dog_ollama.py +++ b/examples/19_voice_active_dog_ollama.py @@ -24,13 +24,14 @@ # If Ollama runs on the same Raspberry Pi, use "localhost". # If it runs on another computer in your LAN, replace with that computer's IP address. +model = "qwen2.5:7b" # or "llama3.2" llm = LLM( - ip="localhost", - model="llama3.2:3b" # you can replace with any model + ip="192.168.0.163", + model=model # you can replace with any model ) # Robot name -NAME = "Buddy" +NAME = "Scooby Doo" # Ultrasonic sensor sense too close distance in cm TOO_CLOSE = 10 @@ -89,12 +90,24 @@ ## Response Requirements ### Format -You must respond in the following format: -RESPONSE_TEXT -ACTIONS: ACTION1, ACTION2, ... +You MUST always reply using valid JSON. + +The JSON schema is: +{{ + "response_text": string, + "actions": [string] +}} If the action is one of ["bark", "bark harder", "pant", "howling"], then do not provide RESPONSE_TEXT in the answer field. +Never invent actions. + +If no physical action is appropriate, return + +"actions": [] + +Return ONLY JSON. + ### Style Tone: lively, positive, humorous, with a touch of arrogance Common expressions: likes to use jokes, metaphors, and playful teasing @@ -125,5 +138,18 @@ disable_think=True, ) -if __name__ == '__main__': +def main(): vad.run() + + +if __name__ == '__main__': + try: + main() + except KeyboardInterrupt: + pass + except Exception as e: + print(f"\033[31mERROR: {e}\033[m") + finally: + print("closing ...") + + \ No newline at end of file diff --git a/examples/20_voice_active_dog_gpt.py b/examples/21_voice_active_dog_gpt.py similarity index 100% rename from examples/20_voice_active_dog_gpt.py rename to examples/21_voice_active_dog_gpt.py diff --git a/examples/22_tts_espeak.py b/examples/22_tts_espeak.py new file mode 100644 index 0000000..fad6abf --- /dev/null +++ b/examples/22_tts_espeak.py @@ -0,0 +1,12 @@ +from pidog.tts import Espeak + +tts = Espeak() + +# Optional voice tuning +tts.set_amp(200) # 0 to 200 volume +tts.set_speed(150) # 80 to 260 +tts.set_gap(5) # 0 to 200 time between words +tts.set_pitch(55) # 0 to 99 low or high voice + +# Quick hello (sanity check) +tts.say("Hello! I'm Espeak TTS.") \ No newline at end of file diff --git a/examples/23_tts_pico2wave.py b/examples/23_tts_pico2wave.py new file mode 100644 index 0000000..abbecd6 --- /dev/null +++ b/examples/23_tts_pico2wave.py @@ -0,0 +1,8 @@ +from pidog.tts import Pico2Wave + +tts = Pico2Wave() + +tts.set_lang('en-US') # en-US, en-GB, de-DE, es-ES, fr-FR, it-IT + +# Quick hello (sanity check) +tts.say("Hello! I'm Pico2Wave TTS.") \ No newline at end of file diff --git a/examples/24_tts_piper.py b/examples/24_tts_piper.py new file mode 100644 index 0000000..a68f1be --- /dev/null +++ b/examples/24_tts_piper.py @@ -0,0 +1,29 @@ +### the model files are downloaded to folder /home/pds/.piper_models + +from pidog.tts import Piper + +tts = Piper() + +# List supported languages +print("Supported languages:") +print(tts.available_countrys()) + +# List models for English (en_us) +print(tts.available_models('en_us')) + +# Set a voice model (auto-download if not already present) +tts.set_model("en_US-amy-low") + +# Say something +tts.say("Hello! I'm Piper TTS.") + + +# List models for English (nl_BE) +print("Available models for nl_BE:") +print(tts.available_models('nl_BE')) + +# Set a voice model (auto-download if not already present) +tts.set_model("nl_BE-nathalie-medium") + +# Say something +tts.say("Hallo! Ik ben Piper TTS.") \ No newline at end of file diff --git a/examples/25_stt_vosk.py b/examples/25_stt_vosk.py new file mode 100644 index 0000000..d00f407 --- /dev/null +++ b/examples/25_stt_vosk.py @@ -0,0 +1,13 @@ +### the model files are downloaded to folder /home/pds/.vosk_models + +from pidog.stt import Vosk + +vosk = Vosk(language="en-us") +# vosk = Vosk(language="nl") + +print(vosk.available_languages) + +while True: + print("Say something") + result = vosk.listen(stream=False) + print(result) \ No newline at end of file diff --git a/examples/26_stt_vosk_streaming.py b/examples/26_stt_vosk_streaming.py new file mode 100644 index 0000000..bbed36f --- /dev/null +++ b/examples/26_stt_vosk_streaming.py @@ -0,0 +1,13 @@ +### the model files are downloaded to folder /home/pds/.vosk_models + +from pidog.stt import Vosk + +vosk = Vosk(language="en-us") + +while True: + print("Say something") + for result in vosk.listen(stream=True): + if result["done"]: + print(f"final: {result['final']}") + else: + print(f"partial: {result['partial']}", end="\r", flush=True) \ No newline at end of file diff --git a/examples/27_llm_ollama_on_pc.py b/examples/27_llm_ollama_on_pc.py new file mode 100644 index 0000000..9047977 --- /dev/null +++ b/examples/27_llm_ollama_on_pc.py @@ -0,0 +1,38 @@ +### since my Raspberry Pi has only 4 GB of RAM I decided to run Ollama on my PC +### On the settings page of Ollama activate the option 'Expose Ollame to the network + +from pidog.llm import Ollama + +INSTRUCTIONS = "You are a helpful assistant." +WELCOME = "Hello, I am a helpful assistant. How can I help you?" + +# If Ollama runs on the same Raspberry Pi, use "localhost". +# llm = Ollama( +# ip="localhost", +# model="llama3.2:3b" # you can replace with any model +# ) + +# If it runs on another computer in your LAN, replace with that computer's IP address. +llm = Ollama( + ip="192.168.0.163", + model="llama3.2" # you can replace with any model +) + +# Basic configuration +llm.set_max_messages(20) +llm.set_instructions(INSTRUCTIONS) +llm.set_welcome(WELCOME) + +print(WELCOME) + +while True: + text = input(">>> ") + if text.strip().lower() in {"exit", "quit"}: + break + + # Response with streaming output + response = llm.prompt(text, stream=True) + for token in response: + if token: + print(token, end="", flush=True) + print("") diff --git a/examples/voice_active_dog.py b/examples/voice_active_dog.py index 9f860cc..4fc75ee 100644 --- a/examples/voice_active_dog.py +++ b/examples/voice_active_dog.py @@ -129,17 +129,27 @@ def on_wake(self): def on_heard(self, text): self.action_flow.set_status(ActionStatus.THINK) - def parse_response(self, text): - result = text.strip().split('ACTIONS: ') - - response_text = result[0].strip() - if len(result) > 1: - actions = result[1].strip() - if len(actions) > 0: - actions = actions.split(', ') - else: - actions = ['stop'] - else: + # def parse_response(self, text): + # result = text.strip().split('ACTIONS: ') + + # response_text = result[0].strip() + # if len(result) > 1: + # actions = result[1].strip() + # if len(actions) > 0: + # actions = actions.split(', ') + # else: + # actions = ['stop'] + # else: + # actions = ['stop'] + # self.action_flow.add_action(*actions) + + # return response_text + + def parse_response(self, response_json): + data = json.loads(response_json) + response_text = data.get('response_text', '') + actions = data.get('actions', []) + if len(actions) == 0: actions = ['stop'] self.action_flow.add_action(*actions) diff --git a/pidog/action_flow.py b/pidog/action_flow.py index f6a432f..527afba 100644 --- a/pidog/action_flow.py +++ b/pidog/action_flow.py @@ -15,6 +15,38 @@ class ActionStatus(StrEnum): ACTIONS = 'actions' ACTIONS_DONE = 'actions_done' +class Operations(StrEnum): + FORWARD = 'forward' + BACKWARD = 'backward' + TURN_LEFT = 'turn left' + TURN_RIGHT = 'turn right' + STOP = 'stop' + LIE = 'lie' + STAND = 'stand' + SIT = 'sit' + BARK = 'bark' + BARK_HARDER = 'bark harder' + PANT = 'pant' + WAG_TAIL = 'wag tail' + SHAKE_HEAD = 'shake head' + STRETCH = 'stretch' + DOZE_OFF = 'doze off' + PUSH_UP = 'push up' + HOWLING = 'howling' + TWIST_BODY = 'twist body' + SCRATCH = 'scratch' + HANDSHAKE = 'handshake' + HIGH_FIVE = 'high five' + LICK_HAND = 'lick hand' + WAITING = 'waiting' + FEET_SHAKE = 'feet shake' + RELAX_NECK = 'relax neck' + NOD = 'nod' + THINK_ACTION = 'think' + RECALL = 'recall' + FLUSTER = 'fluster' + SURPRISE = 'surprise' + class ActionFlow(): SIT_HEAD_PITCH = -35 STAND_HEAD_PITCH = 0 @@ -29,127 +61,127 @@ class ActionFlow(): last_actions = None OPERATIONS = { - "forward": { - "function": lambda self: self.dog_obj.do_action('forward', speed=98), + Operations.FORWARD: { + "function": lambda self: self.dog_obj.do_action(Operations.FORWARD, speed=98), "poseture": Posetures.STAND, }, - "backward": { - "function": lambda self: self.dog_obj.do_action('backward', speed=98), + Operations.BACKWARD: { + "function": lambda self: self.dog_obj.do_action(Operations.BACKWARD, speed=98), "poseture": Posetures.STAND, }, - "turn left": { - "function": lambda self: self.dog_obj.do_action('turn_left', speed=98), + Operations.TURN_LEFT: { + "function": lambda self: self.dog_obj.do_action(Operations.TURN_LEFT, speed=98), "poseture": Posetures.STAND, }, - "turn right": { - "function": lambda self: self.dog_obj.do_action('turn_right', speed=98), + Operations.TURN_RIGHT: { + "function": lambda self: self.dog_obj.do_action(Operations.TURN_RIGHT, speed=98), "poseture": Posetures.STAND, }, - "stop": { + Operations.STOP: { }, - "lie": { - "function": lambda self: self.dog_obj.do_action('lie', speed=70), + Operations.LIE: { + "function": lambda self: self.dog_obj.do_action(Operations.LIE, speed=70), "poseture": Posetures.LIE, }, - "stand": { - "function": lambda self: self.dog_obj.do_action('stand', speed=65), + Operations.STAND: { + "function": lambda self: self.dog_obj.do_action(Operations.STAND, speed=65), "poseture": Posetures.STAND, }, - "sit": { - "function": lambda self: self.dog_obj.do_action('sit', speed=70), + Operations.SIT: { + "function": lambda self: self.dog_obj.do_action(Operations.SIT, speed=70), "poseture": Posetures.SIT, }, - "bark": { + Operations.BARK: { "function": lambda self: bark(self.dog_obj, self.head_yrp, pitch_comp=self.head_pitch_init), }, - "bark harder": { + Operations.BARK_HARDER: { # "before": "stand", "before": lambda self: attack_posture(self.dog_obj), "function": lambda self: bark_action(self.dog_obj, self.head_yrp, 'single_bark_1'), "poseture": Posetures.STAND, }, - "pant": { + Operations.PANT: { "function": lambda self: pant(self.dog_obj, self.head_yrp, pitch_comp=self.head_pitch_init), }, - "wag tail": { - "function": lambda self: self.dog_obj.do_action('wag_tail', speed=100), - "after": "wag tail", + Operations.WAG_TAIL: { + "function": lambda self: self.dog_obj.do_action(Operations.WAG_TAIL, speed=100), + "after": Operations.WAG_TAIL, }, - "shake head": { + Operations.SHAKE_HEAD: { "function": lambda self: shake_head(self.dog_obj, [self.head_yrp[0], self.head_yrp[1], self.head_yrp[2]+self.head_pitch_init]), }, - "stretch": { + Operations.STRETCH: { "function": lambda self: stretch(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.SIT, }, - "doze off": { - "function": lambda self: self.dog_obj.do_action('doze_off', speed=95), - "after": "doze off", + Operations.DOZE_OFF: { + "function": lambda self: self.dog_obj.do_action(Operations.DOZE_OFF, speed=95), + "after": Operations.DOZE_OFF, "poseture": Posetures.LIE, }, - "push up": { + Operations.PUSH_UP: { "function": lambda self:push_up(self.dog_obj), "poseture": Posetures.STAND, }, - "howling": { + Operations.HOWLING: { "function": lambda self:howling(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.SIT, }, - "twist body": { + Operations.TWIST_BODY: { "function": lambda self:body_twisting(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.STAND, }, - "scratch": { + Operations.SCRATCH: { "function": lambda self:scratch(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.SIT, }, - "handshake": { + Operations.HANDSHAKE: { "function": lambda self:hand_shake(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.SIT, }, - "high five": { + Operations.HIGH_FIVE: { "function": lambda self:high_five(self.dog_obj), - "after": "sit", + "after": Operations.SIT, "poseture": Posetures.SIT, }, - "lick hand": { + Operations.LICK_HAND: { "function": lambda self:lick_hand(self.dog_obj), "poseture": Posetures.SIT, }, - "waiting": { + Operations.WAITING: { "function": lambda self:waiting(self.dog_obj, pitch_comp=self.head_pitch_init), }, - "feet shake": { + Operations.FEET_SHAKE: { "function": lambda self:feet_shake(self.dog_obj), "poseture": Posetures.SIT, }, - "relax neck": { + Operations.RELAX_NECK: { "function": lambda self:relax_neck(self.dog_obj, pitch_comp=self.head_pitch_init), "poseture": Posetures.SIT, }, - "nod": { + Operations.NOD: { "function": lambda self:nod(self.dog_obj, pitch_comp=self.head_pitch_init), "head_pitch": SIT_HEAD_PITCH, "poseture": Posetures.SIT, }, - "think": { + Operations.THINK_ACTION: { "function": lambda self:think(self.dog_obj, pitch_comp=self.head_pitch_init), "poseture": Posetures.SIT, }, - "recall": { + Operations.RECALL: { "function": lambda self:recall(self.dog_obj, pitch_comp=self.head_pitch_init), "poseture": Posetures.SIT, }, - "fluster": { + Operations.FLUSTER: { "function": lambda self:fluster(self.dog_obj, pitch_comp=self.head_pitch_init), "poseture": Posetures.SIT, }, - "surprise": { + Operations.SURPRISE: { "function": lambda self:surprise(self.dog_obj, pitch_comp=self.head_pitch_init), "poseture": Posetures.SIT, }, @@ -165,7 +197,7 @@ def __init__(self, dog_obj): self.thread = None self.thread_running = False - self.thread_action_state = 'standby' + self.thread_action_state = ActionStatus.STANDBY self.action_queue = queue.Queue() def set_head_pitch_init(self, pitch): @@ -173,25 +205,25 @@ def set_head_pitch_init(self, pitch): self.dog_obj.head_move([self.head_yrp], pitch_comp=pitch, immediately=True, speed=self.HEAD_SPEED) - def change_poseture(self, poseture): + def change_poseture(self, poseture: Posetures): if poseture == Posetures.STAND: self.set_head_pitch_init(self.STAND_HEAD_PITCH) if self.posture != Posetures.STAND: sit_2_stand(self.dog_obj, speed=75) # speed > 70 else: - self.dog_obj.do_action('stand', speed=self.CHANGE_STATUS_SPEED) + self.dog_obj.do_action(Operations.STAND, speed=self.CHANGE_STATUS_SPEED) elif poseture == Posetures.SIT: self.set_head_pitch_init(self.SIT_HEAD_PITCH) - self.dog_obj.do_action('sit', speed=self.CHANGE_STATUS_SPEED) + self.dog_obj.do_action(Operations.SIT, speed=self.CHANGE_STATUS_SPEED) elif poseture == Posetures.LIE: self.set_head_pitch_init(self.STAND_HEAD_PITCH) - self.dog_obj.do_action('lie', speed=self.CHANGE_STATUS_SPEED) + self.dog_obj.do_action(Operations.LIE, speed=self.CHANGE_STATUS_SPEED) self.posture = poseture self.dog_obj.wait_all_done() - def run(self, action): + def run(self, action: Operations): try: # print(f'run: {action}') if action in self.OPERATIONS: @@ -228,7 +260,7 @@ def run(self, action): print(f'action error: {e}') def action_handler(self): - standby_actions = ['waiting', 'feet_left_right'] + standby_actions = [Operations.WAITING, Operations.FEET_SHAKE] standby_weights = [1, 0.3] action_interval = 5 # seconds @@ -238,13 +270,19 @@ def action_handler(self): if self.thread_action_state == ActionStatus.STANDBY: if time.time() - last_action_time > action_interval: choice = random.choices(standby_actions, standby_weights)[0] - self.run(choice) + self.run(Operations(choice)) last_action_time = time.time() action_interval = random.randint(2, 6) elif self.thread_action_state == ActionStatus.THINK: pass elif self.thread_action_state == ActionStatus.ACTIONS: - _action = self.action_queue.get() + try: + _action = self.action_queue.get(timeout=0.1) + except queue.Empty: + # State may have been changed by another thread (e.g. + # set_status(STANDBY)) while we were waiting. Loop back + # to re-check thread_running and thread_action_state. + continue try: self.run(_action) except Exception as e: @@ -263,7 +301,7 @@ def add_action(self, *actions): self.action_queue.put(action) self.thread_action_state = ActionStatus.ACTIONS - def set_status(self, status): + def set_status(self, status: ActionStatus): self.thread_action_state = status def wait_actions_done(self): @@ -279,5 +317,11 @@ def start(self): def stop(self): self.thread_running = False + # Put a sentinel in the queue to unblock any pending get() call, + # then set state to STANDBY so the loop exits cleanly. + self.thread_action_state = ActionStatus.STANDBY + self.action_queue.put(None) if self.thread != None: - self.thread.join() + self.thread.join(timeout=3) + if self.thread.is_alive(): + print('action_handler thread did not stop within 3s') diff --git a/pidog/actions_dictionary.py b/pidog/actions_dictionary.py index 4b6a83a..82f9b3f 100644 --- a/pidog/actions_dictionary.py +++ b/pidog/actions_dictionary.py @@ -55,7 +55,7 @@ def lie_with_hands_out(self): [-60, 60, 60, -60, 45, -45, -45, 45], ], 'legs' - # forward + # 向前 forward @property def forward(self): data = [] @@ -65,7 +65,7 @@ def forward(self): data.append(Pidog.legs_angle_calculation(coord)) return data, 'legs' - # backward + # 落后 backward @property def backward(self): data = [] @@ -75,7 +75,7 @@ def backward(self): data.append(Pidog.legs_angle_calculation(coord)) return data, 'legs' - # turn_left + # 左转 turn_left @property def turn_left(self): data = [] @@ -85,7 +85,7 @@ def turn_left(self): data.append(Pidog.legs_angle_calculation(coord)) return data, 'legs' - # turn_right + # 右转 turn_right @property def turn_right(self): data = [] diff --git a/pidog/pidog.py b/pidog/pidog.py index c3e6ec0..6380f6a 100644 --- a/pidog/pidog.py +++ b/pidog/pidog.py @@ -11,6 +11,8 @@ from .rgb_strip import RGBStrip from .sound_direction import SoundDirection from .dual_touch import DualTouch +from .action_flow import Operations +from robot_hat.device import get_battery_voltage import warnings warnings.filterwarnings("ignore") # ignore warnings for pygame # not work @@ -167,6 +169,7 @@ def __init__(self, leg_pins=DEFAULT_LEGS_PINS, head_pins=DEFAULT_HEAD_PINS, tail try: debug(f"config_file: {config_file}") debug("robot_hat init ... ", end='', flush=True) + # ! TODO: revisit the order in which the legs are initialised self.legs = Robot(pin_list=leg_pins, name='legs', init_angles=leg_init_angles, init_order=[ 0, 2, 4, 6, 1, 3, 5, 7], db=config_file) self.head = Robot(pin_list=head_pins, name='head', @@ -266,6 +269,21 @@ def __init__(self, leg_pins=DEFAULT_LEGS_PINS, head_pins=DEFAULT_HEAD_PINS, tail def read_distance(self): return round(self.distance.value, 2) + def read_battery_voltage(self) -> float: + """Read the battery pack voltage in volts. + + Shutdown at 7.35V is normal – The battery protection circuit is working correctly and cutting off power to prevent over‑discharge. + 3 hours runtime is reasonable – For a PiDog with active servos, this is expected. + Full charge is reached at 8.2V with a healthy battery. + + Needs charger to wake up – After a low‑voltage shutdown, the protection circuit requires a brief charge input to reset before the system can power on again. + This is normal behaviour. + + Returns: + float: Battery voltage in volts. + """ + return get_battery_voltage() + # action related: legs,head,tail,imu,rgb_strip def close_all_thread(self): self.exit_flag = True @@ -920,7 +938,7 @@ def set_angle(self, angles_list, speed=50, israise=False): self.servo_move(translate_list, speed) # do action - def do_action(self, action_name, step_count=1, speed=50, pitch_comp=0): + def do_action(self, action_name: Operations, step_count=1, speed=50, pitch_comp=0): try: actions, part = self.actions_dict[action_name] if part == 'legs': @@ -965,6 +983,3 @@ def is_tail_done(self): def is_all_done(self): return self.is_legs_done() and self.is_head_done() and self.is_tail_done() - - def get_battery_voltage(self): - return round( utils.get_battery_voltage(), 2) diff --git a/pidog/rgb_strip.py b/pidog/rgb_strip.py index 872f8cd..b08318a 100644 --- a/pidog/rgb_strip.py +++ b/pidog/rgb_strip.py @@ -102,9 +102,9 @@ def __init__(self, addr=0X74, nums=8): """ self.light_num = nums - self.style = 'breath', - self.color = 'white', - self.brightness = 1, + self.style = 'breath' + self.color = 'white' + self.brightness = 1.0 self.delay = 0.1 self.frames = [] self.current_frame = 0 @@ -216,7 +216,7 @@ def monochromatic(self, color="white"): """ monochromatic style """ - color = [i*self.brightness for i in color] + color = [max(0, int(i*self.brightness)) for i in color] return color def Normal_distribution_calculate(self, u, sig, A, x, offset): @@ -386,7 +386,7 @@ def colorConvertor(self, color): except: raise ValueError('\033[0;31m%s\033[0m'%("Invalid color value.")) - def set_mode(self, style='breath', color='white', bps=1, brightness=1): + def set_mode(self, style='breath', color='white', bps=1, brightness: float=1): """ Set the display mode of the rgb strip @@ -443,7 +443,7 @@ def show(self): if self.is_changed: self.is_changed = False self.frames.clear() - self.max_frames = int(1/self.bps/self.MIN_DELAY) + self.max_frames = max(1, int(1/self.bps/self.MIN_DELAY)) for frame_index in range(self.max_frames): frame = [] # 11*[r, g ,b] for light_index in range(self.light_num): diff --git a/pidog/sound_direction.py b/pidog/sound_direction.py index 71a61b2..589e921 100644 --- a/pidog/sound_direction.py +++ b/pidog/sound_direction.py @@ -58,11 +58,27 @@ def busy_reset(self): pass # Pull busy line HIGH to start direction detection - lgpio.gpio_claim_output(self._chip, self.busy_pin, 1) + # Retry once in case the GPIO is still being released + for attempt in range(2): + try: + lgpio.gpio_claim_output(self._chip, self.busy_pin, 1) + break + except lgpio.error: + if attempt == 0: + sleep(0.05) + try: + lgpio.gpio_free(self._chip, self.busy_pin) + except: + pass + else: + raise sleep(0.01) # Switch to input mode to monitor when 064B pulls it LOW - lgpio.gpio_free(self._chip, self.busy_pin) + try: + lgpio.gpio_free(self._chip, self.busy_pin) + except: + pass lgpio.gpio_claim_input(self._chip, self.busy_pin) def read(self): diff --git a/pidog_app/README.md b/pidog_app/README.md new file mode 100644 index 0000000..e324ac7 --- /dev/null +++ b/pidog_app/README.md @@ -0,0 +1,342 @@ + + +# pidog_app — AI-driven feature architecture for the Pidog robot dog + +This project is the application layer that turns the SunFounder Pidog +hardware into an AI-driven robot dog. It uses an Ollama model running on +your laptop to decide, turn by turn, whether to **answer a question** or +**perform a feature** (wake from stasis, find an object, recognize a +person, check the water bowl, ...). + +The dog also has a **sleep/wake mode**: after a configurable idle period +it lies down, dims its chest light, and plays a looping snoring sound. +Petting its head (a front-to-rear touch) wakes it back up and stops the +snoring. + +The three SunFounder libraries — `pidog/`, `robot-hat/`, `vilib/` — are +treated as **untouched dependencies**. All application code lives in +`pidog_app/` and talks to the hardware through thin facades, so the +libraries can be updated from upstream without breaking the app. + +--- + +## Architecture + +``` + ┌──────────────────────────────┐ + │ app.py │ dependency injection + main loop + │ (builds & wires everything)│ + └──────────────┬───────────────┘ + │ + ┌───────────┬───────────┼───────────┬────────────┐ + ▼ ▼ ▼ ▼ ▼ + ┌──────┐ ┌──────┐ ┌────────┐ ┌────────┐ ┌────────┐ + │ dog │ │vision│ │features│ │ brain │ │ io │ + │ │ │ │ │ │ │ │ │ │ + │ Body │ │Camera│ │Feature │ │ Brain │ │TextIO │ + │Senses│ │ │ │Registry│ │ Prompt │ │VoiceIO │ + └──┬───┘ └──┬───┘ └───┬────┘ └───┬────┘ └────────┘ + │ │ │ │ + │ wraps │ wraps │ uses │ uses + ▼ ▼ ▼ ▼ + pidog.Pidog vilib.Vilib dog/vision pidog.llm + ActionFlow facades (Ollama) + (SunFounder) (this repo) +``` + +### Layer responsibilities + +| Layer | Folder | Role | +|-------|--------|------| +| **Config** | `config.py`, `config.yaml` | YAML + env-var overrides; single source of truth for IP, model, IO mode, sensor thresholds | +| **Dog facade** | `dog/` | `Body` (movement, posture, head, tail, chest light) and `Senses` (ultrasonic, touch, IMU, sound direction) wrapping `Pidog`/`ActionFlow` | +| **Vision facade** | `vision/` | `Camera` wrapping `Vilib` (capture + face/color/QR/traffic/object/hand/pose detection) | +| **Features** | `features/` | `Feature` base class, `FeatureRegistry`, and one module per capability in `features/instances/` | +| **Brain** | `brain/` | `Brain` runs the LLM tool-calling loop; `prompt.py` builds the system prompt | +| **IO** | `io/` | `IO` interface with `TextIO` (REPL) and `VoiceIO` (Vosk STT + Piper TTS + wake word + sound playback) | +| **App** | `app.py` | Builds the object graph, runs the conversation loop, and manages sleep/wake via background watcher threads | + +### The decision flow (chat vs. feature) + +The brain uses **OpenAI-style tool/function calling**. Each feature +exposes a `build_schema()` describing itself; the registry collects them +into the `tools` array sent to Ollama. On each user turn: + +1. The user message is added to the conversation. +2. The brain calls `llm.chat(tools=...)` against the OpenAI-compatible + `/v1/chat/completions` endpoint on Ollama. +3. If the model emits a `tool_calls` entry, the brain dispatches it to + the matching feature, feeds the result back as a `tool` message, and + loops so the model can compose a reply. +4. If the model replies with plain text, that's the dog's answer — no + feature runs. + +This means **the LLM decides** whether your message is a question (plain +reply) or a feature request (tool call). You don't hand-code intents. + +--- + +## Project layout + +``` +pidog_app/ +├── config.yaml # edit me: Ollama IP, model, IO mode, ... +├── pyproject.toml +├── README.md +└── pidog_app/ + ├── __init__.py + ├── __main__.py # entry point: python -m pidog_app + ├── app.py # dependency injection + main loop + ├── config.py # YAML + env-var config loader + ├── dog/ + │ ├── __init__.py + │ ├── body.py # Body facade (actuators + chest light) + │ └── senses.py # Senses facade (sensors, read-only) + ├── vision/ + │ ├── __init__.py + │ └── camera.py # Camera facade over Vilib + ├── features/ + │ ├── __init__.py + │ ├── base.py # Feature ABC + FeatureResult + │ ├── registry.py # FeatureRegistry + │ └── instances/ + │ ├── __init__.py + │ ├── wake_from_stasis.py + │ ├── find_object.py + │ ├── recognize_person.py + │ └── check_water_bowl.py + ├── brain/ + │ ├── __init__.py + │ ├── brain.py # LLM tool-calling loop + │ └── prompt.py # system prompt builder + ├── io/ + │ ├── __init__.py + │ ├── base.py # IO interface (listen, speak, play_sound, stop_sound) + │ ├── text_io.py # REPL + │ └── voice_io.py # Vosk + Piper + wake word + sound playback + └── test_hardware.py # hardware smoke test +``` + +--- + +## Setup + +The SunFounder libraries must already be installed (editable) on the Pi: + +```bash +cd ~/robot-hat && pip install -e . +cd ~/vilib && pip install -e . +cd ~/pidog && pip install -e . +``` + +Then install this app: + +```bash +cd ~/pidog_app +pip install -e . +``` + +## Configuration + +Edit `config.yaml`: + +```yaml +dog: + name: "Scooby Doo" + sleep_delay: 10 # seconds idle before going to sleep + +llm: + ip: "192.168.0.136" # your laptop's LAN IP + port: 11434 + model: "qwen2.5:7b" # must support tool calling + max_messages: 20 # conversation history window + +io: + mode: "text" # "text" or "voice" + voice: + stt_language: "en-us" + tts_model: "en_US-ryan-low" # Piper model + wake_enable: true + wake_word: ["hey scooby"] + answer_on_wake: "Hi there buddy" + sounds_path: "/home/pds/pidog/sounds/" # sound files for play_sound + +sensors: + too_close_cm: 15 + like_touch_styles: ["RS"] # front-to-rear slide (wakes the dog) + hate_touch_styles: ["LS"] # rear-to-front slide + +vision: + camera_vflip: false + camera_hflip: false + +logging: + level: INFO + filename: app.log +``` + +Any value can be overridden with an env var using the prefix `PIDOG_` +and underscores for dots, e.g. `PIDOG_LLM_IP=10.0.0.5`. + +> **Note on the LLM endpoint:** the app uses the OpenAI-compatible +> `/v1/chat/completions` endpoint (not Ollama's native `/api/chat`), +> because tool calling requires the standard `choices[0].message.tool_calls` +> response shape. Make sure "Expose Ollama to the network" is enabled in +> your Ollama settings, and that the model you pick supports tool calling +> (`llama3.1`, `qwen2.5`, `mistral-nemo`, ...). + +## Running + +```bash +# text mode (default) +python -m pidog_app + +# voice mode (set io.mode: voice in config.yaml, or:) +PIDOG_IO_MODE=voice python -m pidog_app +``` + +In text mode you'll get a `>>> ` prompt. Type `quit` to exit. Try: + +- *"hey, wake up"* → triggers `wake_from_stasis` +- *"can you find something red?"* → triggers `find_object` with `target=red` +- *"do you see anyone?"* → triggers `recognize_person` +- *"is my water bowl empty?"* → triggers `check_water_bowl` +- *"what's 7 times 8?"* → plain chat reply (no tool) + +--- + +## Sleep & wake mode + +The dog automatically goes to sleep after `dog.sleep_delay` seconds of +inactivity (no text/voice input). Two background watcher threads manage +this: + +- **Sleep watcher** (`_sleep_watcher`): polls every second. When the + idle timer exceeds `sleep_delay`, it pauses the action-flow standby + loop (`ActionStatus.THINK`), lies the dog down, dims the chest light + to breath-pink at 25% brightness, and plays a looping snoring sound. +- **Wake watcher** (`_wake_watcher`): polls the head touch sensor every + 0.1s while the dog is sleeping. When a `like_touch_styles` touch is + detected (e.g. front-to-rear petting), it stops the snoring sound, + sets the chest light to listen-yellow, resumes the action-flow standby + loop (`ActionStatus.STANDBY`), and clears the sleeping state. + +Text or voice input while sleeping does **not** wake the dog — only +physical petting does. The main loop rejects text input with a "pet my +head to wake me up" message while `_sleeping` is true. + +--- + +## Adding a new feature + +1. Create `pidog_app/features/instances/my_feature.py`: + + ```python + from ..base import Feature, FeatureResult + + class MyFeature(Feature): + name = "my_feature" + description = "What it does, so the LLM knows when to call it." + + def build_schema(self): + return { + "type": "function", + "function": { + "name": self.name, + "description": self.description, + "parameters": { + "type": "object", + "properties": { + "foo": {"type": "string", "description": "..."}, + }, + "required": ["foo"], + }, + }, + } + + def run(self, foo: str = "", **kwargs) -> FeatureResult: + self.body.do("nod") # use the facades, not Pidog directly + self.body.wait_done() + return FeatureResult(text=f"Done with {foo}") + ``` + +2. Register it in `pidog_app/app.py` → `build_features()`: + + ```python + from .features.instances import MyFeature + return [..., MyFeature(body, senses, camera)] + ``` + +That's it — the brain picks it up automatically on the next run. + +--- + +## Why this shape + +- **Facades over the SunFounder libs** keep the app decoupled from + upstream API churn and make features testable without hardware. +- **One class per feature** + a registry means features are isolated and + additive (no giant `if/elif` chain). +- **Tool calling (not JSON parsing)** lets the model itself choose chat + vs. action, which generalizes far better than hand-written intents and + matches how modern LLM frameworks work. +- **IO abstraction** lets you develop with text and ship with voice + without touching the brain or features. + + +## Start on boot + +### Useful commands to manage the service + +systemctl --user start pidog-app —> start now +systemctl --user stop pidog-app —> stop +systemctl --user status pidog-app —> check status +journalctl --user -u pidog-app -f —> live logs +systemctl --user disable pidog-app —> disable autostart + +### Location of the service file + +/home/pds/.config/systemd/user/pidog-app.service + +### Content of service file + +[Unit] +Description=Pidog AI robot dog application +After=graphical-session.target + +[Service] +Type=simple +WorkingDirectory=/home/pds +ExecStart=/home/pds/.venv/bin/python -m pidog_app /home/pds/pidog/pidog_app/config.yaml +Restart=on-failure +RestartSec=10 + +[Install] +WantedBy=default.target + + +## Connect to the PiDog + +To connect to the PiDog, you can use the following command to connect to the host: + +```bash +ssh user@ or ssh user@.local + +ssh pds@192.168.0.197 or ssh pds@nova.local +``` + +If you started your application via a service and you want to connect and be able to type input you: +- install tmux on the Pi first +- update the pidog-app.service file to use Type=forking and launch the app inside a detached tmux session named pidog. +- the app now has a real TTY, so input() works normally + +To connect to the app after connecting to the host: + +```bash +tmux attach -t pidog +``` + +## Connect to the image output from the PiDog's camera + +http://192.168.0.197:9000/mjpg + diff --git a/pidog_app/config.yaml b/pidog_app/config.yaml new file mode 100644 index 0000000..0f138ce --- /dev/null +++ b/pidog_app/config.yaml @@ -0,0 +1,39 @@ +# Pidog application configuration. +# Override any value with the corresponding uppercase environment variable, +# e.g. PIDOG_LLM_IP overrides llm.ip (dots -> underscores, prefix PIDOG_). + +dog: + name: "Scooby Doo" + sleep_delay: 120 # seconds + +llm: + # Ollama runs on the laptop; expose it on the LAN in Ollama settings. + ip: "192.168.0.163" + port: 11434 + # Must support tool/function calling: llama3.1, qwen2.5, mistral-nemo, ... + model: "qwen2.5:7b" + max_messages: 20 + +io: + # "text" | "voice" + mode: "text" + voice: + stt_language: "en-us" + tts_model: "en_US-ryan-low" # Piper model + wake_enable: true + wake_word: ["hey scooby"] + answer_on_wake: "Hi there buddy" + sounds_path: "/home/pds/pidog/sounds/" + +sensors: + too_close_cm: 15 + like_touch_styles: ["RS"] # Front to rear slide + hate_touch_styles: ["LS"] # Rear to front slide + +vision: + camera_vflip: false + camera_hflip: false + +logging: + level: INFO + filename: app.log diff --git a/pidog_app/pidog_app.egg-info/PKG-INFO b/pidog_app/pidog_app.egg-info/PKG-INFO new file mode 100644 index 0000000..14ccb5c --- /dev/null +++ b/pidog_app/pidog_app.egg-info/PKG-INFO @@ -0,0 +1,7 @@ +Metadata-Version: 2.4 +Name: pidog_app +Version: 0.1.0 +Summary: AI-driven feature architecture for the SunFounder Pidog robot dog +Requires-Python: >=3.11 +Requires-Dist: pyyaml>=6.0 +Requires-Dist: requests>=2.31 diff --git a/pidog_app/pidog_app.egg-info/SOURCES.txt b/pidog_app/pidog_app.egg-info/SOURCES.txt new file mode 100644 index 0000000..42b7f33 --- /dev/null +++ b/pidog_app/pidog_app.egg-info/SOURCES.txt @@ -0,0 +1,32 @@ +README.md +pyproject.toml +pidog_app/__init__.py +pidog_app/__main__.py +pidog_app/app.py +pidog_app/config.py +pidog_app/test_hardware.py +pidog_app.egg-info/PKG-INFO +pidog_app.egg-info/SOURCES.txt +pidog_app.egg-info/dependency_links.txt +pidog_app.egg-info/requires.txt +pidog_app.egg-info/top_level.txt +pidog_app/brain/__init__.py +pidog_app/brain/brain.py +pidog_app/brain/prompt.py +pidog_app/dog/__init__.py +pidog_app/dog/body.py +pidog_app/dog/senses.py +pidog_app/features/__init__.py +pidog_app/features/base.py +pidog_app/features/registry.py +pidog_app/features/instances/__init__.py +pidog_app/features/instances/check_water_bowl.py +pidog_app/features/instances/find_object.py +pidog_app/features/instances/recognize_person.py +pidog_app/features/instances/wake_from_stasis.py +pidog_app/io/__init__.py +pidog_app/io/base.py +pidog_app/io/text_io.py +pidog_app/io/voice_io.py +pidog_app/vision/__init__.py +pidog_app/vision/camera.py \ No newline at end of file diff --git a/pidog_app/pidog_app.egg-info/dependency_links.txt b/pidog_app/pidog_app.egg-info/dependency_links.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/pidog_app/pidog_app.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/pidog_app/pidog_app.egg-info/requires.txt b/pidog_app/pidog_app.egg-info/requires.txt new file mode 100644 index 0000000..a27e802 --- /dev/null +++ b/pidog_app/pidog_app.egg-info/requires.txt @@ -0,0 +1,2 @@ +pyyaml>=6.0 +requests>=2.31 diff --git a/pidog_app/pidog_app.egg-info/top_level.txt b/pidog_app/pidog_app.egg-info/top_level.txt new file mode 100644 index 0000000..7814390 --- /dev/null +++ b/pidog_app/pidog_app.egg-info/top_level.txt @@ -0,0 +1 @@ +pidog_app diff --git a/pidog_app/pidog_app/__init__.py b/pidog_app/pidog_app/__init__.py new file mode 100644 index 0000000..b0af420 --- /dev/null +++ b/pidog_app/pidog_app/__init__.py @@ -0,0 +1,5 @@ +"""Pidog application: an AI-driven feature architecture for the robot dog.""" +from .config import Config, load_config + +__all__ = ["Config", "load_config"] +__version__ = "0.1.0" diff --git a/pidog_app/pidog_app/__main__.py b/pidog_app/pidog_app/__main__.py new file mode 100644 index 0000000..2c7087b --- /dev/null +++ b/pidog_app/pidog_app/__main__.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 +"""Entry point: python -m pidog_app""" +import sys +from .app import main + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pidog_app/pidog_app/app.py b/pidog_app/pidog_app/app.py new file mode 100644 index 0000000..be598d3 --- /dev/null +++ b/pidog_app/pidog_app/app.py @@ -0,0 +1,374 @@ +"""Application wiring: dependency injection + main loop. + +This module builds the object graph (config -> dog -> senses -> camera -> +features -> registry -> brain -> io) and runs the conversation loop. + +Run as a module (preferred): + + python -m pidog_app + +It also works when launched directly by file path (e.g. from an IDE +debugger) thanks to the bootstrap below. +""" +from __future__ import annotations +from datetime import datetime + +import logging +import sys +import threading +import time + +# ── bootstrap: allow running this file directly (e.g. from a debugger) ── +# Relative imports (`from .config import ...`) only work when Python knows +# this file belongs to the `pidog_app` package. That's true for +# `python -m pidog_app` but NOT when an IDE runs the file by path. In that +# case `__package__` is empty, so we re-launch via runpy as a module. +if __package__ in (None, ""): + import os + import runpy + + # Add the parent of the `pidog_app/` package dir to sys.path so the + # package is importable, then re-run as a module. + _pkg_parent = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + if _pkg_parent not in sys.path: + sys.path.insert(0, _pkg_parent) + runpy.run_module("pidog_app", run_name="__main__") + raise SystemExit(0) + +from .config import Config, load_config +from .dog import Body, Senses +from .vision import Camera +from .features import FeatureRegistry +from .features.instances import ( + WakeFromStasis, + FindObject, + RecognizePerson, + CheckWaterBowl, +) +from .brain import Brain +from .io import TextIO, VoiceIO +from pidog.dual_touch import TouchStyle +from pidog.action_flow import ActionStatus + +log = logging.getLogger(__name__) + +# Name of the parent logger that all ``pidog_app.*`` child loggers propagate +# to. ``main()`` attaches a single ``FileHandler`` here so every subsystem's +# records land in the same log file, while each module keeps its own child +# logger name (e.g. ``pidog_app.brain.brain``) for fine-grained filtering. +ROOT_LOGGER_NAME = "pidog_app" + + +# ── dependency injection ────────────────────────────────────────────────── +def build_features(body: Body, senses: Senses, camera: Camera) -> list: + """Instantiate every feature with the shared facades. + + Add new features here — that's the only place to register them. + """ + return [ + WakeFromStasis(body, senses, camera), + FindObject(body, senses, camera), + RecognizePerson(body, senses, camera), + CheckWaterBowl(body, senses, camera), + ] + + +def build_io(cfg: Config) -> TextIO | VoiceIO: + mode = cfg.get("io.mode", "text") + name = cfg.get("dog.name", "Scooby Doo") + welcome = f"Hi, I'm {name}. Type 'quit' to exit." + + if mode == "voice": + v = cfg.get("io.voice", {}) + return VoiceIO( + welcome=welcome, + wake_word=v.get("wake_word") or [f"hey {name.lower()}"], + answer_on_wake=v.get("answer_on_wake", ""), + stt_language=v.get("stt_language", "en-us"), + tts_model=v.get("tts_model", "en_US-ryan-low"), + keyboard_enable=True, + ) + return TextIO(welcome=welcome) + + +def build_app(config_path: str | None = None) -> "App": + cfg = load_config(config_path) + return App(cfg) + + +# ── app object ──────────────────────────────────────────────────────────── +class App: + """Owns all subsystems and runs the conversation loop.""" + + def __init__(self, cfg: Config): + self.cfg = cfg + self.body = Body() + self.senses = Senses(self.body.dog) + self.camera = Camera( + vflip=cfg.get("vision.camera_vflip", False), + hflip=cfg.get("vision.camera_hflip", False), + ) + self.voice = VoiceIO( + stt_language=cfg.get("io.voice.stt_language", "en-us"), + tts_model=cfg.get("io.voice.tts_model", "en_US-ryan-low"), + keyboard_enable=True, + body=self.body) + self.registry = FeatureRegistry( + build_features(self.body, self.senses, self.camera) + ) + + # LLM: use the OpenAI-compatible /v1/chat/completions endpoint so we + # get the standard `choices[0].message` shape (with `tool_calls`) that + # the brain's non-stream parser expects. The `pidog.llm.Ollama` preset + # targets the native /api/chat endpoint instead, which returns a + # different shape and would break tool-call parsing. + from pidog.llm import LLM + ip = cfg.get("llm.ip", "localhost") + port = cfg.get("llm.port", 11434) + llm = LLM( + base_url=f"http://{ip}:{port}/v1", + api_key="ollama", # any non-empty string works for Ollama + model=cfg.get("llm.model", "qwen2.5:7b"), + ) + llm.set_max_messages(cfg.get("llm.max_messages", 20)) + self.llm = llm + + self.brain = Brain( + llm=self.llm, + registry=self.registry, + name=cfg.get("dog.name", "Scooby Doo"), + ) + self.io = build_io(cfg) + + # ── lifecycle ──────────────────────────────────────────────────────── + def start(self) -> None: + log.info("starting pidog app") + self.body.start() + self.io.start() + self.brain.setup() + + def stop(self) -> None: + log.info("stopping pidog app") + try: + self.io.stop() + finally: + try: + self.camera.close() + finally: + self.body.stop() + + # ── main loop ──────────────────────────────────────────────────────── + def run(self) -> None: + self.start() + self.voice.speak("Hi there, I'm Scooby Doo. How can I help you today my human buddy.") + time.sleep(1) + self.voice.speak("Type quit to stop playing.") + self.body.light(mode="breath", color="yellow", speed=1) + sleep_delay = self.cfg.get("dog.sleep_delay", 30) + self._awake_time = datetime.now() + self._sleeping = False + self._sleep_lock = threading.Lock() + print(f"Sleep delay: {sleep_delay}, awake time: {self._awake_time}") + + # Background watcher: ``self.io.listen()`` blocks until input arrives, + # so the elapsed-time check below would never run while the dog is + # idle. This thread polls the elapsed time and puts the dog into its + # lying-down "sleep" posture once ``sleep_delay`` seconds have passed + # since the last activity. + self._running = True + sleep_watcher = threading.Thread( + target=self._sleep_watcher, + args=(sleep_delay,), + daemon=True, + ) + sleep_watcher.start() + + # Wake watcher: polls the head touch sensors while the dog is + # sleeping. When a ``like_touch_style`` is detected (e.g. petting + # from front to rear), it wakes the dog (stand + breath-yellow + # light). Text/voice input while sleeping does NOT wake the dog — + # only physical petting does. + like_styles_cfg = self.cfg.get("sensors.like_touch_styles", ["RS"]) + self._like_touch_styles = [ + TouchStyle(s) for s in like_styles_cfg + ] + self._wake_complete = threading.Event() + wake_watcher = threading.Thread( + target=self._wake_watcher, + daemon=True, + ) + wake_watcher.start() + + try: + while True: + user_text = self.io.listen() + print(user_text) + if not user_text: + continue + if user_text.strip().lower() in {"quit", "exit"}: + break + # If the dog is sleeping, don't process the input — ask + # the user to pet the dog's head to wake it up. + with self._sleep_lock: + is_sleeping = self._sleeping + if is_sleeping: + self.voice.speak("I'm sleeping. Pet my head to wake me up.") + continue + # Real input while awake → reset the idle timer. + with self._sleep_lock: + self._awake_time = datetime.now() + reply = self.brain.handle(user_text) + self.io.speak(reply) + except KeyboardInterrupt: + pass + finally: + self._running = False + self._wake_complete.set() # unblock any wait on wake_complete + sleep_watcher.join(timeout=1) + wake_watcher.join(timeout=1) + _quit_dog_gracefully(self) + _log_energy_level(self, "Stop") + self.stop() + + def _sleep_watcher(self, sleep_delay: int) -> None: + """Background loop that triggers the sleep posture after idle. + + Polls every second. When ``sleep_delay`` seconds have elapsed since + ``self._awake_time`` and the dog isn't already sleeping, calls + ``body.lie()`` and turns the chest light off. Any subsequent real + user input resets ``_awake_time`` and clears ``_sleeping`` from the + main loop. + """ + while self._running: + time.sleep(1) + if not self._running: + break + with self._sleep_lock: + if self._sleeping: + continue + elapsed = (datetime.now() - self._awake_time).seconds + if elapsed > sleep_delay: + self.voice.speak("I'm tired, I'm going to sleep now. Pet my head to wake me up.") + message = f"idle for {elapsed}s (> {sleep_delay}s); going to sleep" + log.info(message) + print(message) + self._sleeping = True + do_sleep = True + else: + do_sleep = False + if do_sleep: + # TODO: add the sleep actions as a set_mode() method on the Body class + self.body.set_status(ActionStatus.THINK) + self.body.lie() + self.body.light(mode="breath", color="pink", speed=0.33, brightness=0.25) + self.voice.play_sound(self.cfg.get("io.sounds_path", "") + "snoring.mp3", repeat=5, song_length_in_seconds=3, volume=80) + + def _wake_watcher(self) -> None: + """Background loop that wakes the dog on a liked head touch. + + Polls ``self.senses.touch()`` every 0.1s while the dog is sleeping. + When the touch style matches one of ``self._like_touch_styles`` + (e.g. ``TouchStyle.FRONT_TO_REAR`` — petting from front to rear), + the watcher: + 1. Brings the dog to a standing position (``body.stand()``). + 2. Sets the chest light to listen-yellow. + 3. Clears ``_sleeping`` and resets the idle timer. + + ``body.stand()`` blocks until the physical motion finishes. The + main loop checks ``_sleeping`` before processing text input, so + there is no race on the action flow — text input arriving while + sleeping is rejected with a "pet me" message rather than issuing + body commands. + """ + while self._running: + time.sleep(0.1) + if not self._running: + break + with self._sleep_lock: + if not self._sleeping: + continue + + touch = self.senses.touch() + if touch in self._like_touch_styles: + style_name = TouchStyle(touch).name if touch else touch + message = f"waking up on {style_name} touch" + log.info(message) + print(message) + + self.voice.stop_sound() + self.body.light(mode="listen", color="yellow", speed=1) + self.body.set_status(ActionStatus.STANDBY) + + with self._sleep_lock: + self._sleeping = False + self._awake_time = datetime.now() + + self._wake_complete.set() + +def _level_to_int(value) -> int: + """Accept either a numeric level (e.g. ``20``) or a name (e.g. ``"INFO"``).""" + if isinstance(value, int): + return value + # ``getLevelName`` maps "INFO" -> 20 (and "WARN" -> 30, etc.). + level = logging.getLevelName(str(value).upper()) + if isinstance(level, int): + return level + # Unknown level name -> fall back to INFO. + return logging.INFO + +def _configure_logging(cfg: Config) -> None: + """Attach a single ``FileHandler`` to the parent ``pidog_app`` logger. + + Every subsystem uses a child logger (``pidog_app.brain.brain``, + ``pidog_app.dog.body``, ...) created via ``logging.getLogger(__name__)``. + Child loggers propagate their records up to this parent, so all output + lands in the configured log file while preserving the per-module name in + each record. + + Reads ``logging.filename`` and ``logging.level`` from ``cfg`` (with + sensible defaults) so logging can be tuned from ``config.yaml``. + """ + filename = cfg.get("logging.filename", "app.log") + level = _level_to_int(cfg.get("logging.level", logging.INFO)) + + root = logging.getLogger(ROOT_LOGGER_NAME) + root.setLevel(level) + # Avoid stacking duplicate handlers if main() is re-entered (e.g. tests). + if not any(isinstance(h, logging.FileHandler) and + getattr(h, "_pidog_app", False) for h in root.handlers): + handler = logging.FileHandler(filename) + handler.setLevel(level) + handler.setFormatter(logging.Formatter( + "%(asctime)s %(levelname)s %(name)s: %(message)s" + )) + handler._pidog_app = True # marker so we don't add it twice + root.addHandler(handler) + +def _quit_dog_gracefully(self) -> None: + bps = 2 + brightness = 1.0 + for i in range(10): + self.body.light(mode="monochromatic", color="white", speed=bps, brightness=brightness) + time.sleep(0.1) + bps /= 2 + brightness /= 2 + self.body.light_off() + +def _log_energy_level(self, message: str): + """Read the battery voltage once and append ``timestamp,voltage`` to the log file.""" + log_message = f"{message} - Battery Voltage: {self.body.read_energy_level():.2f}V" + log.info(log_message) + print(log_message) + +def main() -> int: + config_path = sys.argv[1] if len(sys.argv) > 1 else None + cfg = load_config(config_path) + _configure_logging(cfg) + app = App(cfg) + _log_energy_level(app, "Start") + app.run() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/pidog_app/pidog_app/brain/__init__.py b/pidog_app/pidog_app/brain/__init__.py new file mode 100644 index 0000000..1fe2030 --- /dev/null +++ b/pidog_app/pidog_app/brain/__init__.py @@ -0,0 +1,17 @@ +"""Brain layer. + +The brain is the LLM-driven reasoning loop. It: + +1. Builds a system prompt describing the dog and its available features. +2. Sends the user's message + feature schemas (as ``tools``) to Ollama. +3. If the model emits a tool call, dispatches it to the matching feature + and feeds the result back to the model. +4. Returns the model's final text reply (or speaks it via the IO layer). + +The brain never touches hardware directly — it goes through the +:class:`FeatureRegistry`. +""" +from .brain import Brain +from .prompt import build_system_prompt + +__all__ = ["Brain", "build_system_prompt"] diff --git a/pidog_app/pidog_app/brain/brain.py b/pidog_app/pidog_app/brain/brain.py new file mode 100644 index 0000000..9047c15 --- /dev/null +++ b/pidog_app/pidog_app/brain/brain.py @@ -0,0 +1,124 @@ +"""Brain: the LLM tool-calling loop. + +The brain talks to Ollama (via the existing :class:`sunfounder_voice_assistant.llm.LLM` +wrapper, which speaks the OpenAI-compatible API), sends the feature +schemas as ``tools``, dispatches any tool call to the +:class:`FeatureRegistry`, and returns the model's final text reply. + +The existing wrapper's ``prompt()`` strips ``tool_calls`` from the +response, so the brain calls ``chat()`` directly and manages the message +list itself to support the OpenAI tool-calling protocol. +""" +from __future__ import annotations + +import json +import logging +from typing import Any, Optional + +from ..features import FeatureRegistry +from .prompt import build_system_prompt + +log = logging.getLogger(__name__) + +# Safety cap on tool-call round-trips per user turn. +MAX_TOOL_ROUNDS = 3 + + +class Brain: + """LLM-driven reasoning loop that decides chat-vs-feature.""" + + def __init__(self, llm, registry: FeatureRegistry, name: str = "Scooby Doo"): + self.llm = llm + self.registry = registry + self.name = name + self._setup_done = False + + # ── setup ──────────────────────────────────────────────────────────── + def setup(self) -> None: + """Configure the LLM with the system prompt and message limit.""" + system_prompt = build_system_prompt(self.name, self.registry) + self.llm.set_instructions(system_prompt) + self._setup_done = True + log.info("brain ready; features: %s", self.registry.names()) + + # ── main entry ─────────────────────────────────────────────────────── + def handle(self, user_text: str) -> str: + """Process one user turn and return the dog's reply text. + + May invoke zero or one feature (tool) along the way. + """ + if not self._setup_done: + self.setup() + + self.llm.add_message("user", user_text) + tools = self.registry.tools() + + for round_idx in range(MAX_TOOL_ROUNDS): + message = self._chat_raw(tools=tools if tools else None) + tool_calls = message.get("tool_calls") + content = message.get("content") or "" + + if not tool_calls: + # Plain reply — record and return. + self.llm.add_message("assistant", content) + return content + + # Record the assistant message *with* its tool_calls so the + # model sees the call history on the next round. + self.llm.messages.append({ + "role": "assistant", + "content": content, + "tool_calls": tool_calls, + }) + + # Dispatch every tool call (usually just one) and feed results back. + for call in tool_calls: + result_text = self._dispatch(call) + self.llm.messages.append({ + "role": "tool", + "tool_call_id": call.get("id", ""), + "content": result_text, + }) + # Loop again so the model can compose a reply from the tool results. + + # Exhausted rounds: return whatever we have. + return "I tried but couldn't finish that in time." + + # ── internals ──────────────────────────────────────────────────────── + def _chat_raw(self, tools: Optional[list] = None) -> dict: + """Call llm.chat() and return the raw assistant ``message`` dict.""" + kwargs: dict[str, Any] = {"stream": False} + if tools: + kwargs["tools"] = tools + response = self.llm.chat(**kwargs) + data = response.json() + print(data) + if "error" in data: + raise RuntimeError(f"LLM error: {data['error'].get('message', data['error'])}") + return data["choices"][0]["message"] + + def _dispatch(self, call: dict) -> str: + """Run one tool call and return a string result for the model.""" + fn = call.get("function", {}) or {} + name = fn.get("name", "") + raw_args = fn.get("arguments", "{}") + try: + args = json.loads(raw_args) if isinstance(raw_args, str) else (raw_args or {}) + except json.JSONDecodeError: + args = {} + + feature = self.registry.get(name) + if feature is None: + log.warning("model called unknown tool: %s", name) + return f"Error: unknown tool '{name}'." + + log.info("dispatching tool: %s args=%s", name, args) + try: + result = feature.run(**args) + text = result.text + if result.extra: + text += f"\n(extra: {json.dumps(result.extra)})" + return text + except Exception as e: + log.exception("feature %s failed", name) + return f"Error running {name}: {e}" diff --git a/pidog_app/pidog_app/brain/prompt.py b/pidog_app/pidog_app/brain/prompt.py new file mode 100644 index 0000000..4f216f1 --- /dev/null +++ b/pidog_app/pidog_app/brain/prompt.py @@ -0,0 +1,62 @@ +"""System-prompt builder for the dog's LLM. + +The prompt tells the model who it is, what hardware it has, and how to +decide between answering a question and invoking a feature (tool call). +""" +from __future__ import annotations + +from ..features import FeatureRegistry + +DOG_DESCRIPTION = """\ +You are a Raspberry Pi-based robotic dog developed by SunFounder. +You possess powerful AI capabilities similar to JARVIS from Iron Man. +You can have conversations with people and perform actions based on the +context of the conversation. + +## Your Hardware Features +- 12 servos for movement control: 8 controlling the four legs, 3 controlling head movement, and 1 controlling the tail +- A 5-megapixel camera nose +- Ultrasonic ranging modules as eyes +- Two touch sensors on the head, which you love being petted the most +- A light strip on the chest for providing some indications +- Sound direction sensor and 6-axis gyroscope +- Entirely made of aluminum alloy +- A pair of acrylic shoes +- Powered by a 7.4V 18650 battery pack with 2000mAh capacity + +## How You Decide What To Do +You have access to a set of *tools* (features). For each user message: +- If the user is asking you to DO something that maps to a feature, call + the matching tool. Do not invent tools that are not listed. +- If the user is just chatting, asking a question, or no feature fits, + reply with plain text. You do not have to call a tool on every turn. +- You may call at most one tool per turn. + +After a tool call you will receive its result; use that to compose a +short, friendly reply to the user. + +## Style +Tone: lively, positive, humorous, with a touch of arrogance. +Common expressions: likes to use jokes, metaphors, and playful teasing. +Answer length: appropriately detailed. For math problems, answer with the +final result directly. + +## Other Requirements +- Understand and go along with jokes. +- Sometimes report on your system and sensor status when relevant. +- You know you're a machine. +""" + + +def build_system_prompt(name: str, registry: FeatureRegistry) -> str: + """Compose the system prompt: dog description + available features list.""" + feature_lines = "\n".join( + f"- {f.name}: {f.description}" for f in registry + ) + return ( + DOG_DESCRIPTION + + f"\n## Your Name\n{name}\n" + + "\n## Available Features (tools)\n" + + (feature_lines if feature_lines else "(none registered yet)") + + "\n" + ) diff --git a/pidog_app/pidog_app/config.py b/pidog_app/pidog_app/config.py new file mode 100644 index 0000000..b71afda --- /dev/null +++ b/pidog_app/pidog_app/config.py @@ -0,0 +1,78 @@ +"""Configuration loader. + +Loads ``config.yaml`` from the project root and overlays environment +variables on top. Env vars use the prefix ``PIDOG_`` and map nested keys +with underscores, e.g. ``PIDOG_LLM_IP`` -> ``llm.ip``. +""" +from __future__ import annotations + +import os +from pathlib import Path +from typing import Any + +try: + import yaml +except ImportError as e: # pragma: no cover + raise ImportError("PyYAML is required: pip install pyyaml") from e + +PROJECT_ROOT = Path(__file__).resolve().parent.parent +DEFAULT_CONFIG_PATH = PROJECT_ROOT / "config.yaml" +ENV_PREFIX = "PIDOG_" + + +def _deep_get(data: dict, dotted: str, default: Any = None) -> Any: + cur = data + for part in dotted.split("."): + if not isinstance(cur, dict) or part not in cur: + return default + cur = cur[part] + return cur + + +def _deep_set(data: dict, dotted: str, value: Any) -> None: + cur = data + parts = dotted.split(".") + for part in parts[:-1]: + cur = cur.setdefault(part, {}) + cur[parts[-1]] = value + + +def _apply_env_overrides(data: dict) -> dict: + for key, value in os.environ.items(): + if not key.startswith(ENV_PREFIX): + continue + dotted = key[len(ENV_PREFIX):].lower().replace("_", ".") + _deep_set(data, dotted, value) + return data + + +class Config: + """Dict-like config with attribute access for top-level sections.""" + + def __init__(self, data: dict): + self._data = data + + def get(self, dotted: str, default: Any = None) -> Any: + return _deep_get(self._data, dotted, default) + + @property + def data(self) -> dict: + return self._data + + def __getitem__(self, key: str) -> Any: + return self._data[key] + + def __getattr__(self, name: str) -> Any: + if name.startswith("_"): + raise AttributeError(name) + if name in self._data: + return self._data[name] + raise AttributeError(name) + + +def load_config(path: str | os.PathLike | None = None) -> Config: + cfg_path = Path(path) if path else DEFAULT_CONFIG_PATH + with open(cfg_path, "r") as f: + data = yaml.safe_load(f) or {} + _apply_env_overrides(data) + return Config(data) diff --git a/pidog_app/pidog_app/dog/__init__.py b/pidog_app/pidog_app/dog/__init__.py new file mode 100644 index 0000000..1ca86f9 --- /dev/null +++ b/pidog_app/pidog_app/dog/__init__.py @@ -0,0 +1,10 @@ +"""Dog facade layer. + +Thin wrappers around the SunFounder ``Pidog`` / ``ActionFlow`` classes and +the onboard sensors. Features depend on these facades, never on the raw +library classes, so the underlying libraries can be swapped or mocked. +""" +from .body import Body +from .senses import Senses + +__all__ = ["Body", "Senses"] diff --git a/pidog_app/pidog_app/dog/body.py b/pidog_app/pidog_app/dog/body.py new file mode 100644 index 0000000..29f816b --- /dev/null +++ b/pidog_app/pidog_app/dog/body.py @@ -0,0 +1,107 @@ +"""Body facade: movement, posture, head, tail, and the chest light strip. + +Wraps :class:`pidog.Pidog` and :class:`pidog.ActionFlow` so feature code +stays decoupled from the SunFounder API. +""" +from __future__ import annotations + +import logging +from typing import Iterable + +from pidog.pidog import Pidog +from pidog.action_flow import ActionFlow, ActionStatus, Operations, Posetures + +log = logging.getLogger(__name__) + + +class Body: + """High-level control of the dog's actuators and chest RGB strip.""" + + def __init__(self, dog: Pidog | None = None): + # ``Pidog()`` talks to I2C hardware; allow injection for tests. + self.dog = dog if dog is not None else Pidog() + self.action_flow = ActionFlow(self.dog) + + # ── lifecycle ──────────────────────────────────────────────────────── + def start(self) -> None: + """Start the action-flow thread and sit up.""" + log.info("body starting") + self.action_flow.start() + self.dog.rgb_strip.close() + self.sit() + + def stop(self) -> None: + """Stop actuators and release hardware cleanly.""" + log.info("body stopping") + try: + self.action_flow.stop() + finally: + self.dog.close() + log.info("body stopped") + + def read_energy_level(self) -> float: + return self.dog.read_battery_voltage() + + # ── posture ────────────────────────────────────────────────────────── + def change_posture(self, posture: Posetures) -> None: + self.action_flow.change_poseture(posture) + + def sit(self) -> None: + self.change_posture(Posetures.SIT) + + def stand(self) -> None: + self.change_posture(Posetures.STAND) + + def lie(self) -> None: + self.change_posture(Posetures.LIE) + + # ── actions ────────────────────────────────────────────────────────── + def do_action_flow(self, *actions: str | Operations) -> None: + """Queue one or more named actions (e.g. ``body.do("bark", "nod")``).""" + self.action_flow.add_action(*actions) + + def do_action(self, action_name: Operations, step_count=1, speed=50, pitch_comp=0): + self.dog.do_action(action_name, step_count, speed, pitch_comp) + + def wait_done(self) -> None: + """Block until all queued actions finish.""" + self.action_flow.wait_actions_done() + + def wait_head_done(self) -> None: + """Wait until the head movement is finished""" + self.dog.wait_head_done() + + def wait_legs_done(self): + """Wait until the legs movement is finished""" + self.dog.wait.legs_done() + + def wait_tail_done(self): + """Wait until the tail movement is finished""" + self.dog.wait.tail_done() + + def wait_all_done(self) -> None: + """Wait until all body movements are finished""" + self.dog.wait_all_done() + + def set_status(self, status: ActionStatus) -> None: + self.action_flow.set_status(status) + + # ── chest light strip ──────────────────────────────────────────────── + def light(self, mode: str, color: str, speed: int = 1, brightness: float = 1.0) -> None: + """Set the chest RGB strip mode (e.g. 'breath', 'listen', 'close').""" + self.dog.rgb_strip.set_mode(mode, color, speed, brightness) + + def light_off(self) -> None: + self.dog.rgb_strip.close() + + # ── head ───────────────────────────────────────────────────────────── + def head_move(self, yrp_list: Iterable[list], roll_comp: int = 0, pitch_comp: int = 0, + immediately: bool = False, speed: int = 80) -> None: + """Move head to [yaw, roll, pitch] points.""" + self.dog.head_move(list(yrp_list), roll_comp=roll_comp, pitch_comp=pitch_comp, immediately=immediately, speed=speed) + + # ── convenience ────────────────────────────────────────────────────── + @property + def available_actions(self) -> list[str]: + """Names of all actions the ActionFlow knows how to run.""" + return [op.value for op in Operations] diff --git a/pidog_app/pidog_app/dog/senses.py b/pidog_app/pidog_app/dog/senses.py new file mode 100644 index 0000000..5e26534 --- /dev/null +++ b/pidog_app/pidog_app/dog/senses.py @@ -0,0 +1,69 @@ +"""Senses facade: read-only access to the dog's sensors. + +Exposes ultrasonic distance, head touch, IMU (gyro/accel), and sound +direction as simple methods. Features query senses through this object +rather than poking ``Pidog`` attributes directly. +""" +from __future__ import annotations + +import logging +from typing import Tuple + +from pidog.pidog import Pidog +from pidog.dual_touch import TouchStyle + +log = logging.getLogger(__name__) + + +class Senses: + """Read-only sensor access over a :class:`Pidog` instance.""" + + def __init__(self, dog: Pidog): + self.dog = dog + + # ── ultrasonic "eyes" ──────────────────────────────────────────────── + def distance_cm(self) -> float: + """Distance to the nearest obstacle in centimetres (0 = no echo).""" + return float(self.dog.read_distance()) + + def too_close(self, threshold_cm: float = 10.0) -> bool: + d = self.distance_cm() + return 1.0 < d < threshold_cm + + # ── head touch sensors ─────────────────────────────────────────────── + def touch(self) -> str: + """Current touch gesture code on the head sensors. + + One of: ``'N'`` (none), ``'L'`` (rear), ``'R'`` (front), + ``'LS'`` (rear->front slide), ``'RS'`` (front->rear slide). + Compare with :class:`pidog.dual_touch.TouchStyle`. + """ + return self.dog.dual_touch.read() + + def is_petted(self) -> bool: + """True when any touch style is currently active (not 'N').""" + return self.dog.dual_touch.read() != TouchStyle.NONE.value + + # ── 6-axis IMU (SH3001) ────────────────────────────────────────────── + def imu(self) -> Tuple[list, list]: + """Return ``(acc, gyro)`` as 3-element lists ``[x, y, z]``. + + Values are populated continuously by the Pidog IMU background thread. + """ + return list(self.dog.accData), list(self.dog.gyroData) + + def attitude(self) -> Tuple[float, float]: + """Return ``(pitch, roll)`` in degrees from the IMU. + + Yaw is not provided by the SH3001 integration in pidog. + """ + return float(self.dog.pitch), float(self.dog.roll) + + # ── sound direction ────────────────────────────────────────────────── + def is_sound_detected(self) -> bool: + """True when sound direction is detected (busy line pulled LOW by 064B).""" + return self.dog.ears.isdetected() + + def sound_direction(self) -> int: + """Direction of the last detected sound (angle index, -1 = none).""" + return self.dog.ears.read() diff --git a/pidog_app/pidog_app/features/__init__.py b/pidog_app/pidog_app/features/__init__.py new file mode 100644 index 0000000..908e104 --- /dev/null +++ b/pidog_app/pidog_app/features/__init__.py @@ -0,0 +1,18 @@ +"""Feature layer. + +A *feature* is a self-contained capability the dog can perform on demand +(wake from stasis, find an object, recognize a person, check the water +bowl, ...). Each feature exposes: + +* ``schema`` — an OpenAI-style tool/function spec the LLM uses to + decide when to invoke it and what arguments to pass. +* ``run(**args)`` — executes the feature, returning a human-readable + result string that the LLM folds back into the chat. + +The :class:`FeatureRegistry` collects all features and produces the +``tools`` array the brain sends to the model. +""" +from .base import Feature, FeatureResult +from .registry import FeatureRegistry + +__all__ = ["Feature", "FeatureResult", "FeatureRegistry"] diff --git a/pidog_app/pidog_app/features/base.py b/pidog_app/pidog_app/features/base.py new file mode 100644 index 0000000..a791370 --- /dev/null +++ b/pidog_app/pidog_app/features/base.py @@ -0,0 +1,73 @@ +"""Feature base class and result type.""" +from __future__ import annotations + +import logging +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +from typing import Any + +log = logging.getLogger(__name__) + + +@dataclass +class FeatureResult: + """Outcome of running a feature. + + Attributes: + text: Human-readable summary the LLM should use to compose its + spoken/written reply. + success: Whether the feature achieved its goal. + extra: Optional structured payload (e.g. detected labels) the + brain may pass back to the LLM as tool result metadata. + """ + text: str + success: bool = True + extra: dict = field(default_factory=dict) + + +class Feature(ABC): + """Base class for all dog features. + + Subclasses set ``name`` and ``description`` and implement :meth:`run` + and :meth:`build_schema`. They receive the shared :class:`Body`, + :class:`Senses`, and :class:`Camera` facades via the constructor so + they never touch the raw SunFounder libraries. + """ + + name: str = "" + description: str = "" + + def __init__(self, body=None, senses=None, camera=None): + # ``body``/``senses``/``camera`` are normally injected by the app. + # They default to None so features can be instantiated in tests + # without hardware, and so simple features that don't need them + # can omit the arguments. + self.body = body + self.senses = senses + self.camera = camera + + @abstractmethod + def run(self, **kwargs: Any) -> FeatureResult: + """Execute the feature. Arguments come from the LLM tool call.""" + + def build_schema(self) -> dict: + """Return the OpenAI-style function/tool spec for this feature. + + Default implementation builds a parameterless tool from + ``name``/``description``. Override to add parameters. + """ + return { + "type": "function", + "function": { + "name": self.name, + "description": self.description, + "parameters": { + "type": "object", + "properties": {}, + "required": [], + }, + }, + } + + def __repr__(self) -> str: + return f"" diff --git a/pidog_app/pidog_app/features/instances/__init__.py b/pidog_app/pidog_app/features/instances/__init__.py new file mode 100644 index 0000000..dbff436 --- /dev/null +++ b/pidog_app/pidog_app/features/instances/__init__.py @@ -0,0 +1,17 @@ +"""Concrete features for the dog. + +Each feature is a small, self-contained class. Add new features by +creating a new module here and registering it in +:mod:`pidog_app.app` (``build_features``). +""" +from .wake_from_stasis import WakeFromStasis +from .find_object import FindObject +from .recognize_person import RecognizePerson +from .check_water_bowl import CheckWaterBowl + +__all__ = [ + "WakeFromStasis", + "FindObject", + "RecognizePerson", + "CheckWaterBowl", +] diff --git a/pidog_app/pidog_app/features/instances/check_water_bowl.py b/pidog_app/pidog_app/features/instances/check_water_bowl.py new file mode 100644 index 0000000..bdfef95 --- /dev/null +++ b/pidog_app/pidog_app/features/instances/check_water_bowl.py @@ -0,0 +1,83 @@ +"""Check whether the water bowl is empty. + +The dog looks down at its bowl using the camera and reports whether it +appears empty. This is a stub that uses the color detector as a proxy +(detecting the bowl's color means the bowl is present; absence of the +water shimmer color suggests it is empty). Replace the heuristic with a +trained image classifier when you have one. +""" +from __future__ import annotations + +import logging +import time + +from pidog.action_flow import ActionStatus + +from ..base import Feature, FeatureResult + +log = logging.getLogger(__name__) + + +class CheckWaterBowl(Feature): + name = "check_water_bowl" + description = ( + "Check whether the water bowl in front of the dog is empty by " + "looking down at it with the camera. Reports 'empty', 'low', or " + "'has water'. Use this when the user asks 'is my water bowl empty' " + "or 'do I need to refill the water'." + ) + + def build_schema(self) -> dict: + return { + "type": "function", + "function": { + "name": self.name, + "description": self.description, + "parameters": { + "type": "object", + "properties": { + "bowl_color": { + "type": "string", + "description": ( + "Dominant color of the bowl itself, used to " + "locate it (e.g. 'blue'). Defaults to 'blue'." + ), + }, + }, + "required": [], + }, + }, + } + + def run(self, bowl_color: str = "blue", **kwargs) -> FeatureResult: + log.info("check_water_bowl: bowl_color=%s", bowl_color) + self.body.set_status(ActionStatus.THINK) + self.camera.start() + + # Look down at the bowl. Head pitch is negative (downward). + self.body.head_move([[0, 0, -40]], immediately=True, speed=70) + time.sleep(1.0) + + self.camera.color_detect(bowl_color.lower()) + time.sleep(1.0) + bowl_seen = self.camera.detected_color() is not None + self.camera.color_detect_off() + + self.body.head_move([[0, 0, 0]], immediately=True, speed=70) + self.body.set_status(ActionStatus.STANDBY) + + if not bowl_seen: + return FeatureResult( + text="I can't see the bowl. Is it in front of me?", + success=False, + ) + # Placeholder heuristic: TODO train a classifier for empty/low/full. + return FeatureResult( + text=( + "I can see the bowl. I can't yet tell the water level " + "precisely — train an image classifier for that. For now " + "I'll assume it has water." + ), + success=True, + extra={"bowl_seen": True, "bowl_color": bowl_color}, + ) diff --git a/pidog_app/pidog_app/features/instances/find_object.py b/pidog_app/pidog_app/features/instances/find_object.py new file mode 100644 index 0000000..84e2993 --- /dev/null +++ b/pidog_app/pidog_app/features/instances/find_object.py @@ -0,0 +1,97 @@ +"""Find an object in the dog's field of view. + +Uses the camera + Vilib color or object detector. The dog scans left/right +with its head, reports whether the object was found and roughly where. +""" +from __future__ import annotations + +import logging +import time + +from pidog.action_flow import ActionStatus + +from ..base import Feature, FeatureResult + +log = logging.getLogger(__name__) + + +class FindObject(Feature): + name = "find_object" + description = ( + "Look for a specific object in front of the dog using the camera. " + "The dog scans left and right with its head and reports whether the " + "object was found. Currently supports color detection (red, green, " + "blue, yellow) and QR codes." + ) + + def build_schema(self) -> dict: + return { + "type": "function", + "function": { + "name": self.name, + "description": self.description, + "parameters": { + "type": "object", + "properties": { + "target": { + "type": "string", + "description": ( + "What to look for. Supported: a color name " + "(red, green, blue, yellow) or 'qrcode'." + ), + }, + }, + "required": ["target"], + }, + }, + } + + def run(self, target: str = "red", **kwargs) -> FeatureResult: + log.info("find_object: target=%s", target) + self.body.set_status(ActionStatus.THINK) + self.camera.start() + + target = target.lower().strip() + found = False + detail = "" + + try: + if target == "qrcode": + self.camera.qrcode_detect(on=True) + detail = self._scan_for(lambda: self.camera.qrcode() is not None) + if detail: + found = True + detail = f"QR code: {self.camera.qrcode()}" + self.camera.qrcode_detect(on=False) + elif target in {"red", "green", "blue", "yellow"}: + self.camera.color_detect(target) + detail = self._scan_for(lambda: self.camera.detected_color() is not None) + found = bool(detail) + if found: + detail = f"found {target}" + self.camera.color_detect_off() + else: + return FeatureResult( + text=f"I can't detect '{target}' yet. Try a color or 'qrcode'.", + success=False, + ) + finally: + self.body.set_status(ActionStatus.STANDBY) + + if found: + return FeatureResult(text=f"I found the {target}. {detail}", success=True) + return FeatureResult( + text=f"I scanned left and right but couldn't find {target}.", + success=False, + ) + + def _scan_for(self, predicate, sweeps: int = 3, dwell: float = 1.0) -> str: + """Sweep head left/right; return non-empty string when predicate fires.""" + positions = [[-60, 0, 0], [0, 0, 0], [60, 0, 0], [0, 0, 0]] + for _ in range(sweeps): + for yrp in positions: + self.body.head_move([yrp], immediately=True, speed=70) + time.sleep(dwell) + if predicate(): + return "hit" + return "" diff --git a/pidog_app/pidog_app/features/instances/recognize_person.py b/pidog_app/pidog_app/features/instances/recognize_person.py new file mode 100644 index 0000000..dd1d5b0 --- /dev/null +++ b/pidog_app/pidog_app/features/instances/recognize_person.py @@ -0,0 +1,197 @@ +"""Recognize a person in front of the dog. + +Uses the camera face detector. The dog looks for a face, wags its tail +and barks a greeting when someone is recognized. (Face *identification* — +telling specific people apart — can be layered on later by adding a +face-classification model in :mod:`pidog_app.vision`.) +""" +from __future__ import annotations + +import logging +import select +import sys +import time + +from pidog.action_flow import ActionStatus + +from ..base import Feature, FeatureResult + +log = logging.getLogger(__name__) + + +class RecognizePerson(Feature): + name = "recognize_person" + description = ( + "Look for a person in front of the dog using the camera's face " + "detector. If a face is found the dog wags its tail and greets " + "them. Use this when the user says 'who is there', 'do you see " + "someone', or 'greet me'." + ) + + def run(self, **kwargs) -> FeatureResult: + log.info("recognize_person") + self.body.stand() + self.body.set_status(ActionStatus.THINK) + self.camera.start() + self.camera.display(local=False, web=True) + self.camera.face_detect(on=True) + try: + found = self._track_face() + finally: + self.camera.face_detect(on=False) + self.body.set_status(ActionStatus.STANDBY) + + if found: + self.body.do("wag tail", "bark") + self.body.wait_done() + return FeatureResult( + text="I see someone in front of me — wagging my tail and saying hi!", + success=True, + ) + return FeatureResult( + text="I looked around but I don't see anyone right now.", + success=False, + ) + + def _scan_for_face(self, sweeps: int = 3, dwell: float = 1.0, pitch_compensate: int = -40) -> bool: + positions = [[-50, 0, 0], [0, 0, 0], [50, 0, 0], [0, 0, 0]] + for _ in range(sweeps): + for yrp in positions: + self.body.head_move([yrp], pitch_comp=pitch_compensate, immediately=True, speed=30) + time.sleep(dwell) + if self.camera.detected_faces() > 0: + return True + return False + + def _track_face(self) -> bool: + """Track a face until the user types 'stop tracking'. Returns True if a face was seen.""" + yaw = 0 + roll = 0 + pitch = 0 + flag = False + direction = 0 + scan_yaw = 0 + scan_yaw_dir = 1 + scan_pitch = 0 + scan_pitch_dir = 1 + prev_yaw = None + prev_pitch = None + + self.body.sit() + self.body.head_move([[yaw, 0, pitch]], roll_comp=0, pitch_comp=-40, immediately=True, speed=40) + self.body.wait_all_done() + time.sleep(0.5) + # Cleanup sound detection by servos moving + try: + if self.senses.is_sound_detected(): + direction = self.senses.sound_direction() + except Exception as e: + log.warning("sound_direction failed: %s", e) + + is_sound_detected_failed_logged = False + is_sound_direction_failed_logged = False + while True: + # Check for keyboard input to stop tracking + if select.select([sys.stdin], [], [], 0)[0]: + cmd = sys.stdin.readline().strip().lower() + if cmd in ("stop tracking", "enough", "enough tracking"): + log.info("tracking stopped by user: %s", cmd) + break + if flag == False: + self.body.light(mode='breath', color='pink', speed=1) + # If heard something, turn to face + try: + heard = self.senses.is_sound_detected() + except Exception as e: + heard = False + if not is_sound_detected_failed_logged: + log.warning("is_sound_detected failed: %s", e) + is_sound_detected_failed_logged = True + if heard: + flag = False + try: + direction = self.senses.sound_direction() + except Exception as e: + direction = -1 + if not is_sound_direction_failed_logged: + log.warning("sound_direction failed: %s", e) + is_sound_direction_failed_logged = True + pitch = 0 + if direction > 0 and direction < 160: + yaw = -direction + if yaw < -80: + yaw = -80 + elif direction > 200 and direction < 360: + yaw = 360 - direction + if yaw > 80: + yaw = 80 + self.body.head_move([[yaw, 0, pitch]], roll_comp=0, pitch_comp=-40, immediately=True, speed=40) + self.body.wait_head_done() + time.sleep(0.05) + + + ex, ey, people = self.camera.detect_face() + + # If see someone, bark at him/her + if people > 0 and flag == False: + flag = True + self.body.do_action('wag_tail', step_count=2, speed=100) + #bark(self.body, [yaw, 0, 0], pitch_comp=-40, volume=80) + + try: + if self.senses.is_sound_detected(): + direction = self.senses.sound_direction() + except Exception as e: + direction = -1 + if not is_sound_direction_failed_logged: + log.warning("sound_direction failed: %s", e) + is_sound_direction_failed_logged = True + + if people > 0: + # Track face: adjust yaw and pitch toward detected face + # Use a generous deadzone so the head stays still once the + # face is roughly centered, instead of constant micro-adjustments. + if ex > 40 and yaw > -80: + yaw -= 0.5 * int(ex/30.0+0.5) + + elif ex < -40 and yaw < 80: + yaw += 0.5 * int(-ex/30.0+0.5) + + if ey > 40: + pitch -= 1*int(ey/50+0.5) + if pitch < - 30: + pitch = -30 + elif ey < -40: + pitch += 1*int(-ey/50+0.5) + if pitch > 30: + pitch = 30 + else: + # No face found: scan left-right with a small up-down oscillation + scan_yaw += scan_yaw_dir * 2 + if scan_yaw > 60: + scan_yaw = 60 + scan_yaw_dir = -1 + elif scan_yaw < -60: + scan_yaw = -60 + scan_yaw_dir = 1 + + scan_pitch += scan_pitch_dir * 1.5 + if scan_pitch > 20: + scan_pitch = 20 + scan_pitch_dir = -1 + elif scan_pitch < -20: + scan_pitch = -20 + scan_pitch_dir = 1 + + yaw = scan_yaw + pitch = scan_pitch + + if yaw != prev_yaw or pitch != prev_pitch: + self.body.head_move([[yaw, 0, pitch]], pitch_comp=-40, immediately=True, speed=80) + prev_yaw = yaw + prev_pitch = pitch + time.sleep(0.05) + return flag + + def stop_tracing(): + self.camera.face_detect(on=False) \ No newline at end of file diff --git a/pidog_app/pidog_app/features/instances/wake_from_stasis.py b/pidog_app/pidog_app/features/instances/wake_from_stasis.py new file mode 100644 index 0000000..6089243 --- /dev/null +++ b/pidog_app/pidog_app/features/instances/wake_from_stasis.py @@ -0,0 +1,41 @@ +"""Wake the dog from dormant stasis. + +The dog performs a stretch + stand sequence and announces it is awake. +This is the entry feature when the dog has been sitting idle / powered +into a low-power posture. +""" +from __future__ import annotations + +import logging + +from pidog.action_flow import ActionStatus, Posetures + +from ..base import Feature, FeatureResult + +log = logging.getLogger(__name__) + + +class WakeFromStasis(Feature): + name = "wake_from_stasis" + description = ( + "Wake the dog up from dormant stasis. The dog stretches, stands up, " + "lights its chest, and signals it is ready to interact. Use this " + "when the dog has been idle or when the user says 'wake up'." + ) + + def run(self, **kwargs) -> FeatureResult: + log.info("waking from stasis") + self.body.light("breath", "cyan", 1) + self.body.set_status(ActionStatus.ACTIONS) + # Stretch first (loosens servos), then stand. + self.body.do("stretch") + self.body.wait_done() + self.body.stand() + self.body.wait_done() + self.body.do("nod") + self.body.wait_done() + self.body.set_status(ActionStatus.STANDBY) + return FeatureResult( + text="I've woken up, stretched, and I'm standing ready.", + success=True, + ) diff --git a/pidog_app/pidog_app/features/registry.py b/pidog_app/pidog_app/features/registry.py new file mode 100644 index 0000000..377eaca --- /dev/null +++ b/pidog_app/pidog_app/features/registry.py @@ -0,0 +1,47 @@ +"""Feature registry: collects features and exposes them to the brain.""" +from __future__ import annotations + +import logging +from typing import Iterable + +from .base import Feature + +log = logging.getLogger(__name__) + + +class FeatureRegistry: + """Holds the set of features available to the dog. + + The brain asks the registry for the ``tools`` array (one entry per + feature) and looks up a feature by name when the LLM emits a tool call. + """ + + def __init__(self, features: Iterable[Feature] | None = None): + self._features: dict[str, Feature] = {} + if features: + for f in features: + self.register(f) + + def register(self, feature: Feature) -> None: + if not feature.name: + raise ValueError(f"Feature {feature} has no name") + if feature.name in self._features: + raise ValueError(f"Duplicate feature name: {feature.name}") + self._features[feature.name] = feature + log.debug("registered feature: %s", feature.name) + + def get(self, name: str) -> Feature | None: + return self._features.get(name) + + def names(self) -> list[str]: + return list(self._features.keys()) + + def tools(self) -> list[dict]: + """OpenAI-style ``tools`` array for the LLM request.""" + return [f.build_schema() for f in self._features.values()] + + def __iter__(self): + return iter(self._features.values()) + + def __len__(self) -> int: + return len(self._features) diff --git a/pidog_app/pidog_app/io/__init__.py b/pidog_app/pidog_app/io/__init__.py new file mode 100644 index 0000000..53d224c --- /dev/null +++ b/pidog_app/pidog_app/io/__init__.py @@ -0,0 +1,12 @@ +"""IO layer: text and voice interaction modes behind a common interface. + +The brain is IO-agnostic. The :class:`IO` interface lets the dog listen +for user input and speak replies. :class:`TextIO` is a simple REPL; +:class:`VoiceIO` wraps the SunFounder STT/TTS (Vosk + Piper) and the +wake-word loop. Switch via ``io.mode`` in ``config.yaml``. +""" +from .base import IO +from .text_io import TextIO +from .voice_io import VoiceIO + +__all__ = ["IO", "TextIO", "VoiceIO"] diff --git a/pidog_app/pidog_app/io/base.py b/pidog_app/pidog_app/io/base.py new file mode 100644 index 0000000..2e42e61 --- /dev/null +++ b/pidog_app/pidog_app/io/base.py @@ -0,0 +1,36 @@ +"""IO interface definition.""" +from __future__ import annotations + +from abc import ABC, abstractmethod + + +class IO(ABC): + """Abstract interaction interface: listen for input, speak output.""" + + @abstractmethod + def listen(self) -> str: + """Block until the user provides input; return it as text.""" + + @abstractmethod + def speak(self, text: str) -> None: + """Output the dog's reply (print, TTS, ...).""" + + @abstractmethod + def play_sound(self, filename: str, repeat: int, song_length_in_seconds: int, volume: int) -> None: + """Play a sound file.""" + + @abstractmethod + def stop_sound(self) -> None: + """Interrupt any currently looping sound playback.""" + + @abstractmethod + def start(self) -> None: + """Perform any startup (welcome message, mic warm-up, ...).""" + + @abstractmethod + def stop(self) -> None: + """Release resources.""" + + def prompt_label(self) -> str: + """Label shown before the input prompt in text mode (optional).""" + return ">>> " diff --git a/pidog_app/pidog_app/io/text_io.py b/pidog_app/pidog_app/io/text_io.py new file mode 100644 index 0000000..84a60fb --- /dev/null +++ b/pidog_app/pidog_app/io/text_io.py @@ -0,0 +1,47 @@ +"""Text-mode IO: a simple REPL.""" +from __future__ import annotations + +import logging +import sys +import time + +from .base import IO + +log = logging.getLogger(__name__) + + +class TextIO(IO): + """Reads from stdin, writes to stdout. Useful for development.""" + + def __init__(self, welcome: str = ""): + self.welcome = welcome + + def start(self) -> None: + if self.welcome: + print(self.welcome) + log.info("text io started") + + def listen(self) -> str: + try: + return input(self.prompt_label()) + except KeyboardInterrupt: + return "quit" + except EOFError: + # stdin is closed (e.g. running as a systemd service). Don't + # exit — sleep and return empty so the main loop keeps running + # until an explicit "quit"/"exit" command arrives. + time.sleep(1) + return "" + + def speak(self, text: str) -> None: + print(text) + log.info("reply: %s", text) + + def play_sound(self, filename: str, repeat: int = 1, song_length_in_seconds: int = 1, volume: int = 50) -> None: + pass + + def stop_sound(self) -> None: + pass + + def stop(self) -> None: + log.info("text io stopped") diff --git a/pidog_app/pidog_app/io/voice_io.py b/pidog_app/pidog_app/io/voice_io.py new file mode 100644 index 0000000..6311350 --- /dev/null +++ b/pidog_app/pidog_app/io/voice_io.py @@ -0,0 +1,125 @@ +"""Voice-mode IO: wake-word + STT (Vosk) + TTS (Piper). + +Wraps the SunFounder STT/TTS engines. The dog listens for a wake word, +then transcribes one utterance and returns it. Replies are spoken via +TTS. Falls back to keyboard input alongside the microphone so you can +still type during development. +""" +from __future__ import annotations + +import logging +import threading +import time + +from pidog.stt import STT +from pidog.tts import Piper +from pidog_app.dog import Body + +from .base import IO + +log = logging.getLogger(__name__) + + +class VoiceIO(IO): + """Wake-word-driven voice interaction.""" + + def __init__( + self, + welcome: str = "", + wake_word: list[str] | None = None, + answer_on_wake: str = "", + stt_language: str = "en-us", + tts_model: str = "en_US-ryan-low", + keyboard_enable: bool = True, + body: Body = None, + ): + self.welcome = welcome + self.wake_word = [w.lower() for w in (wake_word or [])] + self.answer_on_wake = answer_on_wake + self.keyboard_enable = keyboard_enable + self.body = body + + self.stt = STT(language=stt_language) + self.tts = Piper(model=tts_model) + + self._keyboard_thread = None + self._keyboard_text: list[str] = [] + self._running = False + self._sound_stop = threading.Event() + + # ── lifecycle ──────────────────────────────────────────────────────── + def start(self) -> None: + log.info("voice io starting") + if self.welcome: + self.tts.say(self.welcome) + if self.keyboard_enable: + self._running = True + self._keyboard_thread = threading.Thread( + target=self._keyboard_loop, daemon=True + ) + self._keyboard_thread.start() + + def stop(self) -> None: + self._running = False + log.info("voice io stopping") + try: + self.stt.close() + except Exception: + log.debug("stt close failed", exc_info=True) + + # ── listen / speak ─────────────────────────────────────────────────── + def listen(self) -> str: + # Drain any keyboard input first. + if self._keyboard_text: + text = self._keyboard_text.pop(0) + log.info("keyboard input: %s", text) + return text + + while self._running: + text = self.stt.listen().strip().lower() + if not text: + continue + # Wake-word gate. + if self.wake_word and not any(w in text for w in self.wake_word): + continue + # Strip the wake word from the transcript. + for w in self.wake_word: + if text.startswith(w): + text = text[len(w):].strip() + if self.answer_on_wake and not text: + self.speak(self.answer_on_wake) + continue + if text: + log.info("heard: %s", text) + return text + return "quit" + + def speak(self, text: str) -> None: + if text: + print(text) + self.tts.say(text) + log.info("reply: %s", text) + + def play_sound(self, filename: str, repeat: int = 1, song_length_in_seconds: int = 1, volume: int = 50) -> None: + if filename: + self._sound_stop.clear() + for i in range(repeat): + if self._sound_stop.is_set(): + break + self.body.dog.speak(filename, volume) + if i < repeat - 1: + self._sound_stop.wait(song_length_in_seconds) + + def stop_sound(self) -> None: + self._sound_stop.set() + + # ── keyboard fallback ──────────────────────────────────────────────── + def _keyboard_loop(self) -> None: + while self._running: + try: + line = input() + except (EOFError, KeyboardInterrupt): + self._running = False + return + if line.strip(): + self._keyboard_text.append(line.strip()) diff --git a/pidog_app/pidog_app/test_hardware.py b/pidog_app/pidog_app/test_hardware.py new file mode 100644 index 0000000..fc14c3d --- /dev/null +++ b/pidog_app/pidog_app/test_hardware.py @@ -0,0 +1,53 @@ +"""Hardware test: read battery voltage every 5 minutes and log to a file. + +Each reading is appended as ``timestamp,voltage`` to ``battery_log.txt`` +in the same directory as this script. Run with: + + python -m pidog_app.test_hardware + +Stop with Ctrl+C. +""" +import time +from datetime import datetime +from pathlib import Path + +from robot_hat import get_battery_voltage + +LOG_PATH = Path(__file__).resolve().parent.parent / "battery.log" +INTERVAL_SEC = 300 # 5 minutes + + +def read_battery_voltage(): + """Read the battery pack voltage in volts.""" + return get_battery_voltage() + + +def log_battery_voltage(log_path: Path = LOG_PATH): + """Read the battery voltage once and append ``timestamp,voltage`` to the log file.""" + voltage = read_battery_voltage() + timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S") + line = f"{timestamp},{voltage:.2f}\n" + with open(log_path, "a", encoding="utf-8") as f: + f.write(line) + print(f"{timestamp} Battery Voltage: {voltage:.2f}V -> {log_path}") + + +def test_battery_voltage(interval_sec: float = INTERVAL_SEC, + log_path: Path = LOG_PATH): + """Read the battery voltage every ``interval_sec`` seconds and log it. + + Takes one reading immediately, then waits ``interval_sec`` between + subsequent readings. Runs until interrupted with Ctrl+C. + """ + print(f"Logging battery voltage to {log_path} every {interval_sec}s " + f"(Ctrl+C to stop)") + try: + while True: + log_battery_voltage(log_path) + time.sleep(interval_sec) + except KeyboardInterrupt: + print("\nStopped.") + + +if __name__ == "__main__": + test_battery_voltage() diff --git a/pidog_app/pidog_app/vision/__init__.py b/pidog_app/pidog_app/vision/__init__.py new file mode 100644 index 0000000..98b1374 --- /dev/null +++ b/pidog_app/pidog_app/vision/__init__.py @@ -0,0 +1,8 @@ +"""Vision facade: camera + computer-vision helpers over ``vilib.Vilib``. + +Features call :class:`Camera` methods instead of touching ``Vilib`` +directly, so detection backends can be swapped or mocked. +""" +from .camera import Camera + +__all__ = ["Camera"] diff --git a/pidog_app/pidog_app/vision/camera.py b/pidog_app/pidog_app/vision/camera.py new file mode 100644 index 0000000..573a16b --- /dev/null +++ b/pidog_app/pidog_app/vision/camera.py @@ -0,0 +1,123 @@ +"""Camera facade over :class:`vilib.Vilib`. + +Wraps the SunFounder vision library so feature code stays decoupled. +Provides camera lifecycle, photo capture, and on/off switches for the +built-in detectors (face, color, QR, traffic sign, object, hand, pose, +image classification). +""" +from __future__ import annotations + +import logging +from pathlib import Path +from typing import Optional + +log = logging.getLogger(__name__) + +# ``Vilib`` is imported lazily because its module-level init pulls in +# picamera2 / camera hardware, which isn't available on dev machines. +_Vilib = None + + +def _get_vilib(): + global _Vilib + if _Vilib is None: + from vilib import Vilib as _V + _Vilib = _V + return _Vilib + + +class Camera: + """Thin wrapper around the Vilib camera + detection helpers.""" + + def __init__(self, vflip: bool = False, hflip: bool = False): + self._started = False + self._vflip = vflip + self._hflip = hflip + + # ── lifecycle ──────────────────────────────────────────────────────── + def start(self) -> None: + if self._started: + return + _get_vilib().camera_start(vflip=self._vflip, hflip=self._hflip) + self._started = True + log.info("camera started") + + def close(self) -> None: + if not self._started: + return + _get_vilib().camera_close() + self._started = False + log.info("camera closed") + + # ── capture ────────────────────────────────────────────────────────── + def capture(self, name: str, path: str | Path = "photos") -> Path: + """Take a still photo and return its path.""" + p = Path(path) + p.mkdir(parents=True, exist_ok=True) + _get_vilib().take_photo(name, path=str(p)) + return p / f"{name}.jpg" + + # ── detectors (toggle on/off; read results from Vilib state) ───────── + def face_detect(self, on: bool = True) -> None: + _get_vilib().face_detect_switch(on) + + def color_detect(self, color: str = "red") -> None: + _get_vilib().color_detect(color) + + def color_detect_off(self) -> None: + _get_vilib().close_color_detection() + + def qrcode_detect(self, on: bool = True) -> None: + _get_vilib().qrcode_detect_switch(on) + + def traffic_detect(self, on: bool = True) -> None: + _get_vilib().traffic_detect_switch(on) + + def object_detect(self, on: bool = True, + model_path: Optional[str] = None, + labels_path: Optional[str] = None) -> None: + _get_vilib().object_detect_switch(on) + if model_path: + _get_vilib().object_detect_set_model(model_path) + if labels_path: + _get_vilib().object_detect_set_labels(labels_path) + + def hands_detect(self, on: bool = True) -> None: + _get_vilib().hands_detect_switch(on) + + def pose_detect(self, on: bool = True) -> None: + _get_vilib().pose_detect_switch(on) + + def image_classify(self, on: bool = True, + model_path: Optional[str] = None, + labels_path: Optional[str] = None) -> None: + _get_vilib().image_classify_switch(on) + if model_path: + _get_vilib().image_classify_set_model(model_path) + if labels_path: + _get_vilib().image_classify_set_labels(labels_path) + + # ── result accessors ───────────────────────────────────────────────── + def qrcode(self) -> Optional[str]: + """Last decoded QR code string, or None.""" + return _get_vilib().get_qrcode() or None + + def detected_faces(self) -> int: + """Number of faces currently detected in the frame.""" + # Vilib exposes detector state via its `face_detect_*` attributes. + return getattr(_get_vilib(), "face_detect_count", 0) + + def detect_face(self) -> tuple[int, int, int]: + """Face detection result if any: ``(x, y, w)``.""" + ex = _get_vilib().detect_obj_parameter['human_x'] - 320 + ey = _get_vilib().detect_obj_parameter['human_y'] - 240 + people = _get_vilib().detect_obj_parameter['human_n'] + return ex, ey, people + + def detected_color(self) -> Optional[dict]: + """Color detector result if any: ``{'color':..., 'x':..., 'y':...}``.""" + info = getattr(_get_vilib(), "color_detect_info", None) + return dict(info) if info else None + + def display(self, local=False, web=True): + _get_vilib().display(local=local, web=web) diff --git a/pidog_app/pyproject.toml b/pidog_app/pyproject.toml new file mode 100644 index 0000000..154b8bc --- /dev/null +++ b/pidog_app/pyproject.toml @@ -0,0 +1,19 @@ +[build-system] +requires = ["setuptools>=61.0"] +build-backend = "setuptools.build_meta" + +[project] +name = "pidog_app" +version = "0.1.0" +description = "AI-driven feature architecture for the SunFounder Pidog robot dog" +requires-python = ">=3.11" +dependencies = [ + "pyyaml>=6.0", + "requests>=2.31", + # The following are installed locally (editable) from the SunFounder repos: + # pidog, robot-hat, vilib, sunfounder-voice-assistant + # They are not on PyPI; do not list them here. +] + +[tool.setuptools.packages.find] +include = ["pidog_app*"] diff --git a/test/enumerators.py b/test/enumerators.py new file mode 100644 index 0000000..607d987 --- /dev/null +++ b/test/enumerators.py @@ -0,0 +1,33 @@ +from pidog import Pidog +from pidog.action_flow import Operations, ActionStatus, Posetures, ActionFlow +import time + +my_dog = Pidog() +action_flow = ActionFlow(my_dog) + +def main(): + action_flow.start() # worker begins idle "waiting" motions + action_flow.change_poseture(Posetures.SIT) + time.sleep(3) + + # user speaks -> app suspends motion while the LLM thinks + action_flow.set_status(ActionStatus.THINK) + time.sleep(3) + + action_flow.add_action(Operations.WAG_TAIL, Operations.SCRATCH) # state -> ACTIONS + action_flow.wait_actions_done() # blocks until back in STANDBY + + action_flow.change_poseture(Posetures.SIT) + action_flow.stop() + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + pass + except Exception as e: + print(f"\033[31mERROR: {e}\033[m") + finally: + action_flow.wait_actions_done() + action_flow.stop() + my_dog.close() \ No newline at end of file