diff --git a/.clang-format b/.clang-format index 5ab089a..358e344 100644 --- a/.clang-format +++ b/.clang-format @@ -1,5 +1,6 @@ --- Language: Cpp +BasedOnStyle: LLVM AccessModifierOffset: -4 AlignAfterOpenBracket: BlockIndent AlignConsecutiveBitFields: @@ -26,20 +27,25 @@ IncludeCategories: SortPriority: 1 - Regex: '^".*"' SortPriority: 2 -IncludeIsMainRegex: '$' +# No include is treated as the main header, so that sorting preserves the existing +# grouping instead of promoting the header matching the source file name +IncludeIsMainRegex: 'NO_MAIN_INCLUDE' IndentCaseLabels: true IndentExternBlock: NoIndent IndentPPDirectives: BeforeHash IndentRequiresClause: false IndentWidth: 4 IndentWrappedFunctionNames: true +InsertNewlineAtEOF: true KeepEmptyLinesAtTheStartOfBlocks: false +LineEnding: LF PackConstructorInitializers: NextLine PointerAlignment: Left SeparateDefinitionBlocks: Always ShortNamespaceLines: 0 -SortIncludes: Never +SortIncludes: CaseSensitive SpaceInEmptyBlock: true SpacesBeforeTrailingComments: 2 +Standard: Latest TabWidth: 4 ... diff --git a/.clang-tidy b/.clang-tidy index a2ec1be..33139c4 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,4 +1,13 @@ --- +# llvm-include-order enforces LLVM's own ordering, which contradicts the project's +# .clang-format rules; clang-format is the single authority on include order here. +# +# The trailing group disables checks for constructs this project cannot contain. They match +# nothing, so they only cost time scanning the standard library and the Cube headers: +# string_view, coroutines, Objective-C, GoogleTest, the LLVM internal APIs, std::bind, +# iostreams, std::auto_ptr and std::random_shuffle (both removed in C++17), and printf. +# It also drops clang-analyzer-core.FixedAddressDereference, which every memory mapped +# peripheral access looks like by construction. Checks: -*, bugprone-*, @@ -14,8 +23,8 @@ Checks: google-*, llvm-*, -llvm-header-guard, + -llvm-include-order, misc-*, - -misc-include-cleaner, -misc-use-anonymous-namespace, modernize-*, -modernize-use-nodiscard, @@ -25,6 +34,19 @@ Checks: -readability-duplicate-include, -readability-magic-numbers, -readability-math-missing-parentheses, + -bugprone-stringview-nullptr, + -cppcoreguidelines-no-suspend-with-lock, + -google-objc-*, + -google-upgrade-googletest-case, + -llvm-prefer-isa-or-dyn-cast-in-conditionals, + -llvm-prefer-register-over-unsigned, + -llvm-twine-local, + -modernize-avoid-bind, + -modernize-deprecated-ios-base-aliases, + -modernize-replace-auto-ptr, + -modernize-replace-random-shuffle, + -modernize-use-std-print, + -clang-analyzer-core.FixedAddressDereference, HeaderFileExtensions: - '' - h @@ -36,9 +58,17 @@ ImplementationFileExtensions: - cc - cpp - cxx -HeaderFilterRegex: '' -FormatStyle: none +HeaderFilterRegex: '(include|config)/.*\.(hpp|tpp)$' +ExcludeHeaderFilterRegex: '(_deps|external)/' +FormatStyle: file CheckOptions: + # The vendor HAL module headers are not standalone; they require stm32xx_hal.h to have + # pulled in stm32xx_hal_def.h first, so include-cleaner must not suggest or remove them. + # This is why micras_hal includes the generated main.h, which reaches the right one for the + # family it was generated for, instead of naming a module header of its own. main.h is on the + # list for the same reason: every symbol behind it resolves to a header already listed here, so + # nothing is credited to main.h and it would otherwise be reported as unused. + misc-include-cleaner.IgnoreHeaders: 'stm32.*;core_cm.*;cmsis.*;system_stm32.*;lsm6dsv.*;main\.h' readability-identifier-length.MinimumVariableNameLength: 2 readability-identifier-length.IgnoredVariableNames: ^[nxyz]$ readability-identifier-length.MinimumParameterNameLength: 2 diff --git a/.docker/Dockerfile b/.docker/Dockerfile new file mode 100644 index 0000000..fb7479c --- /dev/null +++ b/.docker/Dockerfile @@ -0,0 +1,65 @@ +# syntax=docker/dockerfile:1.27 + +# >>> host stage >>> +############################################################################### +# Host tools: shared byte for byte by micras-lib, micras-simulation and # +# MicrasFirmware, whose CI compares the lines between the two markers # +############################################################################### +FROM ubuntu:26.04 AS host + +# The compilers and the clang tooling are pinned to a major version, so that a distribution update +# can move neither the generated code nor the formatting rules and the enabled check set +ARG HOST_GCC_VERSION=15 +ARG HOST_CLANG_VERSION=22 + +# The apt caches are kept in BuildKit cache mounts instead of the image layers, so the hook that +# deletes the downloaded packages after every install has to go +RUN rm -f /etc/apt/apt.conf.d/docker-clean + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + ca-certificates \ + cmake \ + doxygen \ + git \ + graphviz \ + ninja-build \ + python3 \ + python3-matplotlib \ + python3-numpy \ + "gcc-${HOST_GCC_VERSION}" \ + "g++-${HOST_GCC_VERSION}" \ + "clang-${HOST_CLANG_VERSION}" \ + "clang-format-${HOST_CLANG_VERSION}" \ + "clang-tidy-${HOST_CLANG_VERSION}" + +WORKDIR /project +# <<< host stage <<< + +############################################################################### +# The simulator: what the viewer, the video and their checks need on top # +############################################################################### +FROM host AS simulator + +ARG HOST_CLANG_VERSION=22 + +# GLFW and the OpenGL and EGL development files for the viewer and the video, Mesa's software renderer +# for a machine with no GPU, Xvfb for a window with no display, and ffmpeg, which encodes the video. +# clang's own sanitizer headers let clang-tidy read a sanitized build: MuJoCo's headers include them. +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + apt-get update && \ + apt-get install -y --no-install-recommends \ + "libclang-rt-${HOST_CLANG_VERSION}-dev" \ + ffmpeg \ + libegl-dev \ + libegl-mesa0 \ + libgl-dev \ + libgl1-mesa-dri \ + libglfw3-dev \ + libglx-mesa0 \ + libopengl-dev \ + xauth \ + xvfb diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..c59f5e6 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,64 @@ +name: Build test lint and check the simulator + +on: + push: + pull_request: + workflow_dispatch: + +permissions: + contents: read + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.ref }} + cancel-in-progress: true + +# A push to a branch of this repository is checked once by its push run, so a pull request only runs +# here when it comes from a fork +jobs: + host: + name: ๐Ÿงช Build, tests, format, lint and checks + runs-on: ubuntu-latest + if: github.event_name != 'pull_request' || github.event.pull_request.head.repo.full_name != github.repository + timeout-minutes: 60 + steps: + - name: ๐Ÿ”€ Checkout + uses: actions/checkout@v7 + + - name: ๐Ÿ‹ Build the simulator image + run: docker build --file .docker/Dockerfile --target simulator --tag micras-simulation:ci .docker + + - name: ๐Ÿ—ƒ๏ธ Restore the fetched dependencies + uses: actions/cache@v6 + with: + path: | + build/host-ci/_deps + build/host-ci/CMakeFiles/fc-stamp + key: deps-host-ci-${{ hashFiles('cmake/third_party.cmake', 'cmake/mujoco.cmake') }} + + # One long lived container runs every step below, as the user that owns the checkout + - name: ๐Ÿ“ฆ Start the simulator container + run: > + docker run --detach --name simulator --user "$(id -u):$(id -g)" --env HOME=/tmp + --volume "$PWD:/project" --workdir /project micras-simulation:ci sleep infinity + + - name: โš™๏ธ Configure + run: docker exec simulator cmake --preset host-ci + + - name: ๐Ÿ”จ Build + run: docker exec simulator cmake --build --preset host-ci + + - name: ๐Ÿงช Run the tests + run: docker exec simulator ctest --preset host-ci + + - name: ๐ŸŽจ Check the formatting + run: docker exec simulator cmake --build --preset host-ci --target micras_sim_format_check + + - name: ๐Ÿšจ Lint + run: docker exec simulator cmake --build --preset host-ci --target micras_sim_lint + + - name: โœ… Check the simulator + run: docker exec simulator cmake --build --preset host-ci --target micras_sim_check + + - name: ๐Ÿงน Stop the simulator container + if: always() + run: docker rm --force simulator diff --git a/.gitignore b/.gitignore index 9c91abd..d194acb 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,4 @@ runs/ build/ .cache/ __pycache__/ -.omc/ compile_commands.json -third_party/_deps/ diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index f3f8645..0000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "MicrasFirmware"] - path = MicrasFirmware - url = git@github.com:Team-Micras/MicrasFirmware.git - branch = feature/sim-harness diff --git a/CLAUDE.md b/CLAUDE.md index a6ebc95..ddea796 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,71 +1,112 @@ # micras-simulation `README.md` is how to use this. This file is why it is built the way it is: the -invariants that hold it together, the subtleties that look like bugs and are -not, and the measurements behind the numbers in `config/constants.hpp`. +invariants that hold it together and the subtleties that look like bugs and are +not. A robot's own measurements live with its target. -Linux only, by decision. `just` is the entry point; there is no Makefile. +Linux only, by decision. CMake is the entry point: presets configure, and every +recipe is a CMake target over a bash script that takes its paths as arguments, so +no script guesses a build layout. There is no task runner. ## The rule everything else serves **A run is reproducible.** Two runs of the same binary with the same arguments -produce byte-identical `data.csv` and `meta.json`. Nothing in `sim/` may make +produce byte-identical `data.csv` and `meta.json`. Nothing in the engine may make the firmware's view of the world depend on wall time, on a window being open, on -a client being connected, or on how fast the machine is. +a client being connected, or on how fast the machine is. Noise is part of the +world and is seeded: `--seed` picks the stream, `--ideal` turns it off. -This is enforced, not hoped for. `just check` runs the same scenario headless -and with a window, headless and with a bridge, and compares the two CSVs as -bytes; and it compares the three standard scenarios against the recorded -baselines in `baseline/`. +This is enforced, not hoped for. `micras_sim_check` runs the toy target's +scenario twice, headless and with a window, with a video, and with a bridge nobody +connects to, and compares each run with the plain one as bytes +(`scripts/check_invariance.sh`, `tools/compare_run.py`). -Two invariants govern the baselines: +Baselines are summaries, not recordings. A robot's single baseline, +`targets//baseline/`, holds one `summary.json` per checked run: the hash of +`data.csv`, the state timeline, and a handful of numbers with their tolerances +(`tools/baseline.py`). +On the machine that recorded it the hash matches; on another it will not, because +the compiler, libm and the MuJoCo build all move the last bits, and the summary is +what is compared. Two rules: -- **A refactoring must not move a byte.** If `data.csv` differs from - `baseline/v2`, the refactoring is wrong. Revert it. Never re-record the - baseline to make a diff go away. -- **An extension only appends columns.** Every row's prefix up to the first new - column stays byte-identical, and the previous baseline is kept and compared - with `--columns-subset`, so old recordings stay comparable forever. Note what - that check is: a header prefix. New columns therefore have to go at the very - end โ€” appending a pool variable would shift every `proxy_*` column right and - break it. +- **A refactoring must not move a byte.** On one machine, a refactoring leaves + the hash where it was. If it moves, the refactoring is wrong. +- **Never re-record to make a difference go away.** A change of behavior is + recorded over the baseline (`micras_sim_toy_record_baseline`, or a robot + project's own recording target), with the reason in the commit; git keeps the + earlier ones. -## Architecture +## Layers -Five libraries. `micras_sim_app` sits on top of all of them; `micras_sim_view` -links only the core, so nothing about drawing can reach the recording layer: +Two layers here, and a third in the project of each robot: ``` -micras_sim_core world, clock, serial bus, proxy state, run loop, firmware thread -micras_sim + scenario, telemetry, recording (links the firmware shadow) -micras_sim_view window, control panel, video recorder (GLFW/ImGui/EGL, on core) -micras_sim_bridge + WebSocket server, monitor bridge (IXWebSocket) -micras_sim_app + CLI, application wiring, crash reporter +engine/, view/, bridge/, app/ the simulator: knows no robot and no HAL +targets/toy/ the reference target: no HAL, every feature of the engine +a robot's project its target, bindings and robot.toml, over micras-lib's host HAL ``` -`src/main.cpp` is one call; `Application::main` is the `try`. The application is the only layer -that knows a run can have a window, so it is the only one that links the view. +`micras_sim_check_generic` greps everything outside `targets/` for names that +belong to a robot and fails on any; only the files shared with micras-lib, which +name the repositories that carry them, and the script itself are not scanned. The +root CMakeLists adds every folder of `targets/` that has a CMakeLists; nothing +above `targets/` names one. -`MICRAS_VIEWER`, `MICRAS_VIDEO`, `MICRAS_BRIDGE` and `MICRAS_TESTS` are all ON -by default and all must still compile when OFF; `just check-options` configures -every one of them off as part of the gate, because stale stubs are otherwise -only found by whoever first tries to build without a GPU. +Four engine libraries. `micras_sim_app` sits on top; `micras_sim_view` links only +the engine, so nothing about drawing can reach the recording layer: + +``` +micras_sim_engine world, clock, run loop, firmware thread, devices, arenas, + robot description and model, scenarios, recording +micras_sim_view window, declarative panel and overlay, video (GLFW/ImGui/EGL) +micras_sim_bridge WebSocket server, monitor bridge (IXWebSocket) +micras_sim_app CLI, application wiring, crash reporter; the Target interface +``` + +A robot target implements `Target` (`app/include/micras/sim/app/target.hpp`): its +name, folder, firmware commit, loop period, options, robot file, ground truth +columns and program, and `wire()`, which binds its devices to the host ports and +returns what it adds to the run (`Wiring` in `app/include/micras/sim/app/wiring.hpp`: +columns, variables, panel, overlay, scenario hooks). Its `main` is one call to +`micras::sim::run`. The program receives the `FirmwareThread` it runs on, so a +target with no HAL yields on it directly and needs no pointer kept from `wire()`. +The README lists the headers a target may include; they are the public API. + +`MICRAS_SIM_VIEWER`, `MICRAS_SIM_VIDEO`, `MICRAS_SIM_BRIDGE`, `MICRAS_SIM_TESTS` +and `MICRAS_SIM_TARGETS` must all still compile when OFF; `micras_sim_check_options` +configures every one of them off, because stale stubs are otherwise only found by +whoever first tries to build without a GPU. The first three are ON by default; the +tests and the robot targets are ON only when the simulator is the top-level project. + +### Added to another project + +A project can add the simulator with `add_subdirectory(... EXCLUDE_FROM_ALL)` and +build its own target against `micras::sim_app`. Nothing leaks into it: the style +targets are the simulator's own (`micras_sim_format`, `micras_sim_format_check`, +`micras_sim_lint`) and exist only at top level; the build type is defaulted only at +top level, and never forced; the dependencies' switches are normal variables, not +cache entries; paths are anchored to `PROJECT_SOURCE_DIR`/`PROJECT_BINARY_DIR`; the +engine libraries require C++23 of whoever links them; the tests and `targets/` are +off. `MICRAS_SIM_TOOLS_DIR` and `MICRAS_SIM_MAZES_DIR` (internal cache variables) +name the tools and the mazes for the project's own recipes. ### The run loop `Simulation::run` drives `IRunListener`s. Order per tick: ``` -on_before_tick -> RunControl{RUN, QUIT} crash reporter, scenario, monitor, pause -firmware.run_until_yield() the firmware thread runs one loop -world.step(steps_per_tick) 2 x 0.000521 s = 1042 us +on_before_tick -> RunControl{RUN, QUIT} crash reporter, scenario, monitor +firmware.run_until_yield() the firmware runs until its timer crosses a step +device->actuate() motors, fan: from what the firmware wrote +world.step(steps_per_tick) 125 us, one MuJoCo step clock.advance() -on_after_tick monitor, telemetry, recorder, video, viewer +device->sample() encoders, IMU, wall sensors, ADCs, link +on_after_tick event log, recorder, video, viewer ``` -Listeners run in registration order, which `Application` fixes. `Telemetry` -overrides only `on_after_tick`, so the pool values a row carries are decoded -after the tick they belong to and before the recorder writes it. +Listeners run in registration order, which `Application` fixes. Devices sample +after the step, so what the firmware reads in tick N+1 is the world at the end of +tick N: the true boundary, not a rounding choice. `has_finished()` is checked both before `before_tick()` and after `run_until_yield()`, so a firmware that has exited gets no physics step and no @@ -73,38 +114,48 @@ CSV row. A `FinishGuard` runs `finish()` however `run()` leaves, including through an exception from a listener; without it a thrown listener would leave the firmware thread parked on the handoff with nobody left to wake it. -### The two threads +### Time: the host timer hands over + +The firmware keeps its own notion of time through `micras_hal`'s timer, and the +host backend's `Clock` (micras-lib) is the only place that time exists. **Every read of the +timer costs one microsecond of simulated time**, since a read on the robot takes +time too and a loop that polls the timer must see it advance. When a read +crosses the end of a 125 us step, the clock calls its handover, which is the +firmware thread's `yield_tick()`: the world advances one step and the read +returns after it. So a busy wait runs the world exactly as long as it waits, the +firmware's 125 us loop is one step, and the time inside one iteration is the +number of timer reads times the quantum: deterministic, but not a measurement of +anything. There are no spin guards and no special cases. -The firmware runs `micras::Micras` in its own thread. This is not concurrency: -it is a **strict handoff**, a mutex and a condition variable around a `Turn` -flag, and the two threads are never runnable at the same time. The firmware -thread runs until it calls `yield_tick()`, then blocks; the simulation thread -advances physics, then wakes it. A ThreadSanitizer build reported no race, which -is what the design predicts; nothing in the repo runs that build routinely. +### The two threads -The thread exists because the firmware's loop is a loop: `Micras::update()` -busy-waits on its own stopwatch, and there is no way to return from the middle -of it. Everything else โ€” the window, the panel, the bridge callbacks that matter -โ€” is drained on the simulation thread. +The firmware runs its own `main`, compiled under another name, in its own +thread. This is not concurrency: it is a **strict handoff**, a mutex and a +condition variable around a `Turn` flag, and the two threads are never runnable +at the same time. The firmware thread runs until the host timer yields, then +blocks; the simulation thread advances physics, then wakes it. A +ThreadSanitizer build reported no race, which is what the design predicts; +nothing in the repo runs that build routinely. `finish()` latches `stopping` before checking `joinable`, and if the program refuses to stop it says so on stderr and calls `std::_Exit`. It does not detach: a detached thread running over a destroyed world is worse than a loud exit. `yield_tick()` returns false instead of parking when the thread is stopping -during unwinding, which is how a firmware test that throws gets to leave. - -The thread body is called `thread_body()` and not `main()`, because the hardware -test executables compile the firmware's `main` under `-Dmain=micras_test_main`, -which would rename a member called `main` too. +during unwinding. The thread body is called `thread_body()` and not `main()`, +because the firmware's `main` is renamed with `-Dmain=...`, which would rename a +member called `main` too. ### The viewer draws on the simulation thread -There is no render thread, and therefore no snapshot and no mutex around the -proxy state โ€” the snapshot only ever existed to protect a render thread that was -never built. `MujocoViewer` draws inside `on_after_tick` and blocks inside -`on_before_tick` while paused. Rendering dominated wall time when it was last -measured, roughly 70 % of it; `--viewer-fps` is the lever, and the figure is an -estimate rather than something the repo records. +There is no render thread. `MujocoViewer` draws inside `on_after_tick` and +blocks inside `on_before_tick` while paused; `--viewer-fps` is the lever on wall +time. The panel and the overlay are declarative: a target returns a +`PanelSpec` of buttons, switches, lamps and readouts and an `OverlaySpec` of +lines, and the view draws them without knowing the robot. The first touch of a +board control hands the run over: the scenario stops driving the inputs and the +run is marked `interactive`. Panel input uses ImGui **edges** +(`IsItemActivated`/`IsItemDeactivated`); writing every frame from the held state +would overwrite a scripted press. EGL and GLX contexts cannot both be current on one thread. `--viewer --video` therefore requires that `VideoRecorder` make its EGL context current in @@ -112,271 +163,145 @@ therefore requires that `VideoRecorder` make its EGL context current in GLFW context current first. Without that pair the run dies with `BadAccess X_GLXMakeCurrent` and leaves an empty CSV. -## The shadow proxy layer - -`config/` and `include/micras/proxy/` shadow the real `micras_proxy`. Include -order matters: `config` and `include` come before `MicrasFirmware/include` so -the firmware picks up ours. `MicrasFirmware/src/main.cpp`, `micras_hal` and -`micras_proxy` are excluded from the build. - -Proxies are constructed as members of `Micras`, initialised at their -declarations from the `*_config` globals in `config/target.hpp`. There is no -constructor to inject anything into. So each `Config` carries a -`sim::SimulationContext*`, and `SimulationContext::instance()` is a -function-local static, so it is constructed on first use and never before -whoever depends on it. Exactly three places name it: `config/target.hpp`, once -per `Config`; `Application`'s member initialiser, which is where the process's -one context is picked up; and the context's own file, for the yield helper the -proxies call. **No proxy and nothing else in `sim/` may call it** โ€” they receive -the facet they need -(`MujocoWorld&`, `Clock&`, `SerialBus&`, `ProxyState&`) through the `Config`. A -proxy built without a context, or before a model is loaded, throws at -construction. Tests build their own `SimulationContext` and pass it through the -same field. - -`tests/unit/shadow_conformance_test.cpp` instantiates every shadow `Config` with -the firmware's own designated initialisers, which is what catches a field that -drifted (it already caught `Storage::Config`'s `start_page` against -`start_sector`). - -`tools/check_config_drift.py` diffs `config/constants.hpp` and -`config/target.hpp` against the firmware's field by field and fails on anything -not in the allowlist at the top of the script. Entries still marked `TODO` there -are known divergences not yet written into the `constants.hpp` `@note`. - -### Stopwatch, the one that bites - -`Stopwatch` reports **simulated** time, and it is honest: it returns what the -clock actually says. That alone deadlocks, because the firmware busy-waits on it -while physics is frozen. So it has spin guards, and they are deliberately -asymmetric: `elapsed_time_us` yields on the **second** repeated read at the same -simulated instant (`max_repeated_reads = 2`), `elapsed_time_ms` only on the -**fourth** (`max_repeated_ms_reads = 4`), because the firmware legitimately reads -the same millisecond twice in one tick when it classifies a button release. -`sleep_us` yields until the target passes. - -`reset_us` and `reset_ms` move `counter`, which is the point of them. What -deliberately **survives a reset** are `last_read_us` and the two spin counters. -This looks wrong and is load-bearing: `Fan::update` resets its own stopwatch every call, so a counter -that reset with it would never reach the threshold, and `test_fan` would spin -forever. The constructor seeds `counter = now_us - us_per_tick`, so the -firmware's first loop reads the nominal period instead of zero. +## The host HAL + +The host backend of `micras_hal` lives in +[micras-lib](https://github.com/Team-Micras/micras-lib) (`micras_hal/host/`), +with the SPI device slot and the models of the SPI chips (`micras_proxy/models/`); +its README describes the port registry keyed by the Cube handles, the slot and +the models. The simulator includes no HAL. A robot's target binds the backend's +ports to the engine's devices, and nothing in the backend knows a physics engine. +A port the firmware used that no binding claimed is counted, named on stderr at +the end of the run and written to `meta.json` as `unbound_ports`; a gate expects +zero, and so it does of `watchdog_expiries` and `emergency_stops`, which the +backend's `Mcu` counts instead of resetting a process. + +## The toy target + +`targets/toy/` exists so that the simulator is checked without any real robot: +it uses every device, the firmware thread's handover (its program yields a tick +on the thread it is given, once per 1 ms loop), a command line option, the +panel, the overlay and the scenario hooks, and it is what the invariance checks +and the simulator's baseline run on. It has no HAL: its devices write into a +plain struct its program reads, which is safe because the two threads never run +at once. Its `robot.toml` is `tests/models/tiny_robot.toml` with the name, a +125 us timestep and a second wall sensor, since the wall sensors fire in two +groups. Its baseline runs start from their output directory with a copy of the +scenario, so a summary records no machine's paths. + +## Presets, recipes and sanitizers + +`host` is Debug with the address and undefined behavior sanitizers, and is where +the tests and `micras_sim_check` run; `host-release` (RelWithDebInfo) is for runs +and baselines; `host-ci` is `host` with warnings as errors. A run's bytes do not +depend on the build type (a Debug sanitized toy run equals the RelWithDebInfo +baseline). The invariance check turns leak detection off for the window and the +video runs only: the system's GL and font libraries keep their caches until exit. + +The simulator's checks (`cmake/checks.cmake`, `scripts/`) and a robot's recipes +(`targets//CMakeLists.txt`, `targets//scripts/`) exist only when the +simulator is the top-level project, and every script takes its paths as +arguments. + +## The robot description + +A target's `robot.toml` is the robot's physical truth, written by hand from the +CAD, the board and the datasheets. Every value is either a number or +`{ value, source }` naming where it came from, and `RobotDescription` rejects a +missing key, an unknown key, a wrong type and an unknown schema version. The +engine generates the MJCF from it (`robot_mjcf`) and composes it with the arena +(`Maze::mjcf`, attached under the prefix `maze_`); the composed model is saved to +`/model.xml` and hashed into `meta.json`. What each number of a real robot +rests on is written next to its target. ## The CSV -85 columns: the tick, the simulated time and the firmware's own loop time, then -33 of MuJoCo ground truth, 29 from the firmware pool, and 20 of what crossed the -proxy boundary. +One row per tick, or per `--record-every` ticks. The engine's block first: the +tick, the simulated time, the body pose and velocity. Then the robot's ground +truth columns, then the firmware's own monitoring variables, then each device's +columns. Two sources naming the same column is an error at the first row. + +The first row is the first one due once every column source is ready +(`ColumnSource::ready`). A firmware's variables may exist only once its robot is +constructed, so a firmware whose start-up waits on its chips starts its CSV +that many ticks late. - A MuJoCo free joint splits its six velocity dofs across two frames, so the linear columns are `vx_world`, `vy_world`, `vz_world` and the angular one is `wz_body`. `v_forward` is the world linear velocity projected on the body - forward axis, `-vx_world*sin(yaw) + vy_world*cos(yaw)`; the model's forward - axis is body +y. -- `left_penetration` and `right_penetration` are `nan` on ticks where the geom - had no contact at all. `0` means "in contact, exactly touching". -- The `proxy_*` block is what the firmware read and wrote at the **start** of - the tick, so those sensor columns are one tick behind the ground truth on the - same row. That is the true boundary, not a rounding choice. -- `grid_pose` is the cell the robot is *moving into*, so it legitimately leads - the ground-truth cell by one cell for most of a run. `grid_pose_side` follows - `nav::Side` (RIGHT=0, UP=1, LEFT=2, DOWN=3) and `fsm_state` follows - `Micras::State` (INIT=0, IDLE=1, WAIT_FOR_RUN=2, RUN=3, WAIT_FOR_CALIBRATE=4, - CALIBRATE=5, ERROR=6). - -Columns the firmware never feeds are **not** in the CSV. The battery voltage and -the LED/ARGB/buzzer block were removed once measured: this build never reads the -battery, and the only call that reaches those proxies is the `buzzer->update()` -in `Micras::update`, so the columns were constant. `ProxyState` still carries `interface_output`, because the panel shows -it. + forward axis, which for this robot is body +x. +- `*_penetration` is `nan` on ticks where the geom had no contact at all. `0` + means "in contact, exactly touching". +- The variable columns come from the firmware's `VariablePool`, read through a + read-only accessor, and are named from its own variable names. State ids come + from the firmware; it has no names for them, so the target lists the names and + the build checks that there is one for each state. The device columns are + what the simulated hardware produced: the IMU samples, the `wall_*` readings, + the `motor_*_voltage` the bridge applied, `pack_voltage`. + +What the recording decimates, the event log does not: collisions and state +changes are detected every tick and written to `meta.json`'s `events`. A +collision is a chassis geom touching anything but the floor after having been +clear of everything for 50 ms; a robot pressed against a wall makes and breaks +contact every few steps, and that is one collision, not hundreds. The CSV is flushed after every row, and a SIGSEGV/SIGABRT handler writes `firmware crashed at tick N` to stderr before re-raising, so a firmware crash still leaves an analysable run behind. -## Telemetry and the bus - -`BluetoothSerial` is an in-process byte queue. At tick 0 the harness injects one -`SERIAL_VARIABLE_MAP_REQUEST` and names the pool columns from the response. The -firmware clears its whole receive deque after framing one packet, so **never -inject more than one packet per tick** โ€” the `SerialBus` enforces exactly that, -which is also what keeps a burst from a connected monitor out of the run's -determinism. - -The map reports types through `core::type_name`, so they arrive fully qualified -(`micras::nav::GridPose`, `micras::nav::State`, alongside plain `float` and -`unsigned char`). The decoder matches on the last component only and expands custom serializables into one column per field. - -The monitor bridge is the same bus with a socket on it. Incoming bytes land in a -mutex-guarded `PacketFramer` on an IXWebSocket thread and are drained on the -simulation thread in `on_before_tick`; the tick's output goes out as one binary -frame in `on_after_tick`. A port already taken is a warning, not a failure. - -That one-packet-per-tick rule is what keeps a burst from a connected monitor out -of the run, but note what the gate actually proves: `check-monitor` compares a -bridged run **with nobody connected** against a plain one. The bridged run that -does have a client attached is not compared against anything, so immunity to a -talking monitor is enforced by construction and not yet by a test. - -## Real versus stub proxies - -- Backed by MuJoCo: `Motor`/`Locomotion`, `Fan`, `RotarySensor`, `Imu`, - `TWallSensors<4>`. -- Deterministic but synthetic: `Stopwatch` (sim clock), `Battery` (constant), - `BluetoothSerial` (in-process), `Storage` (in-memory, blank every run). -- Stubs recording the last value: `Led`, `TArgb<2>`, `Buzzer`, `Button`, - `TDipSwitch<4>`, `TTorqueSensors<2>` (always 0). - -`Button` and `TDipSwitch` read `ProxyState::interface_input`, written by the -scenario or by the panel. `config/target.hpp` uses the firmware button delays, -`long_press_delay = 500` and `extra_long_press_delay = 2000`, and -`Button::update` classifies on `elapsed_time_ms` strictly greater than each, so -the scenario holds the button for 250 ms, 501 ms and 2001 ms. - -The panel hands over rather than fights: the first touch of any board control -sets `driven_by_human`, and `Scenario::on_before_tick` returns early from then -on. Panel input uses ImGui **edges** (`IsItemActivated`/`IsItemDeactivated`), not -`IsItemActive`; writing every frame from the held state overwrites the scripted -press and silently turns an `extra_long` into a `SHORT_PRESS`. - -## The firmware submodule - -`MicrasFirmware/` sits on `feature/sim-harness`, branched from `ad34254`. -**Edit it only with intent**, in the firmware's own style -(`.clang-format`, Doxygen on every declaration), and only with changes that make -sense on the real robot too. What is there now: - -- `Micras::run` sets `navigation_failed` when `push_exploring` queues nothing, - stops locomotion and logs; `RunState` turns that into `State::ERROR` the way - `check_crash` does. Before this the firmware popped an empty deque and crashed. -- `ActionQueuer::pop` returns the pre-built stop action instead of popping an - empty deque, because every caller dereferences the result unchecked. -- New monitoring variables, appended after the existing ones so no id shifts: - `FSM State`, `Grid Pose`, `Odometry State`, `Left/Right Command`, - `Linear/Angular PID Integral`, `Odometry Linear Raw`, plus the accessors they - need (`core::Fsm::get_current_state`, `core::PidController::get_error_acc`, - `nav::SpeedController::get_pid_error_acc`, - `nav::Odometry::get_raw_linear_velocity`). -- The Butterworth fixes described below. - -`nav::TMaze::get_next_goal` returns the default `GridPose{{0, 0}, RIGHT}` when no -side qualifies: every open neighbour must beat the initial `current_cost` (0x1FFF), -so a cell whose open neighbours are all still at `max_cost` yields the default and -`push_exploring` then matches none of its four cases. That is reached through -odometry drift writing walls into the wrong cells, not through a bug in the search. +## The link and the bridge + +The firmware's radio is a UART behind `hal::UartDma`; the `SerialLink` device +moves bytes between it and the engine's `SerialBus` no faster than the configured +baud rate, and the bus keeps whatever is pending (up to 64 KiB, counting what it +drops in `serial_dropped_bytes`). There is no framing in the simulator: the +firmware's `comm::Link` does its own. + +The monitor bridge carries raw bytes both ways. Incoming bytes land in a +mutex-guarded buffer on an IXWebSocket thread and are handed to the bus on the +simulation thread in `on_before_tick`; each tick's output goes out as one binary +frame in `on_after_tick`, through a bounded queue and a sender thread, so a +client that stops reading loses frames (`bridge_dropped_frames`) instead of +stalling the run. A port already taken is a warning, not a failure. + +Note what the gate proves: the bridge check compares a bridged run **with nobody +connected** against a plain one. A run a client talks to is marked `interactive` +and is not reproducible, by definition. + +## Scenarios + +A scenario is a TOML file: the arena, the duration, the seed, the start pose, +timed events (`press` an input for a time, `set` it, `send` a link command) and a +stop condition on a firmware variable. CLI flags override what they name. Starts +go through the robot's real paths: a press of the button starts it, exactly as +on the robot. A scenario never touches the physics, so a +scripted run is one a human could have driven. ## Style -The firmware's `.clang-format` and `.clang-tidy`, with the tidy header filter -anchored to this repo. Doxygen on every declaration. **No comments inside -function bodies** โ€” if something needs explaining, it goes in an `@note` on the -declaration, where a reader finds it before reading the code. Every `NOLINT` -names its check and says why. `just lint` is clean and stays clean. - -## State of the tuning (2026-09-16) - -`models/robot_v2.xml` plus the retuned `config/constants.hpp` **reach the goal**: -a 120 s explore on `models/maze.txt` touches a goal cell at 38.8 s, returns to -`{0, 0}` and goes IDLE at 94.6 s. The run that follows, started automatically, -still trips `ERROR` at 98.2 s. The same binary on `models/maze2.txt` (the 2019 -Portugal qualifier) does not reach the goal and dies at 69.2 s, so the tuning is -not maze independent yet. - -What was measured and fixed, in order: - -1. **Butterworth sampling frequency and normalisation.** `core::ButterworthFilter` - normalised the analog prototype by `f_c` rather than by `2 * pi * f_c`, and - every call site took the 100 Hz default `sampling_frequency` while being fed at - `loop_frequency` (959.7 Hz). The two errors compounded into a factor of - `loop_frequency / (2 * pi * 100) = 1.527`: the real cutoffs were 1.527 times - the configured numbers, i.e. the filters were *faster* than intended. Both are - fixed in the firmware, `sampling_frequency` is now an explicit field with no - default, and every `filter_cutoff` was restated as the cutoff actually in - effect, so the tuning carried over unchanged and the fix is behaviour-neutral. - `FollowWall::check_posts` reads `get_adc_reading`, which is unfiltered, so - filter lag never affected post detection at all. -2. **Speed PIDs closed.** Both were `kp = 0`, i.e. identically zero. They now - carry the firmware structure rescaled by the feed-forward gain of each axis. - The decisive term was `ki`: `TurnAction` is open loop in time, so the 4 % - steady-state angular error that `ki = 1` could not close inside a 1.6 s pivot - came straight out as a 21 deg heading error on every 180 deg turn. Executed - turn angle 0.886 -> 0.987 of the commanded one. -3. **Post correction.** Left at `post_reference = 0.44 * cell_size` and - `post_threshold = 400`. Lowering the threshold is a clear negative: - `-d(adc_reading)/d(distance)` on the side sensors peaks at 32 (left) and 456 - (right) over a whole 120 s run, so at 400 the correction fires about four times - and at 100 or below it fires constantly and *injects* error. Thresholds - 400/100/50/30/20 give a first ERROR at 98.2/45.8/40.8/40.8/40.8 s on maze 1 and - the goal is only reached at 400. The reference is what is wrong, not the - sensitivity, and fixing it needs a measurement of where the sim post edge - actually falls. -4. **Chassis clearance.** The `base` geom is raised 2.5 mm in `robot_v2.xml`, - putting the chassis 3 mm above the floor instead of 0.5 mm. Floor scraping went - from 63 % of ticks to under 1 %. -5. **Exploring curve radius.** `exploring.max_centrifugal_acceleration` 1.0 -> 2.0 - shrinks the exploring curve from a 90 mm to a 45 mm radius. A 90 mm curve does - not fit a 167.4 mm corridor with a 66 mm wide chassis. -6. **Linear PID `ki` 10 -> 40.** `Micras::run` calls `speed_controller.reset()` at - every action boundary, and an exploring action is 45 to 180 mm long, so the - integral only gets 0.16 to 0.6 s to close the 5 % steady-state speed error - before being wiped. That error is not cosmetic: `TurnAction` holds a constant - linear speed for a fixed time, so at -5.5 % the exploring curve displaces - 41.7 mm instead of the 45 mm `ActionQueuer` assumes, and `MoveAction` measures - distance travelled rather than position in the cell, so the missing millimetres - never come back. First ERROR 22.3 -> 29.0 s, maximum odometry error 131 -> - 57 mm. Raising the feed-forward `linear_speed` instead changed nothing - measurable โ€” the PID simply unwound the same amount โ€” and was reverted. -7. **The lateral loop was bang-bang.** This was the one that mattered. - `FollowWall` returns `state.velocity.linear * pid(left_error - right_error)`; - around the corridor centre that error is 5.34 per metre of lateral offset, so - with `kp = 30` the proportional term alone reached the old `saturation = 1.0` at - 6.2 mm of offset. Past that the loop was pure bang-bang at +-v rad/s and - limit-cycled with a 1.9 s period, half a metre of travel per swing: the robot - crossed the centre line with its full heading error still on it, entered the - next curve 8 deg off and walked into the wall. `saturation` 1.0 -> 4.0 keeps the - loop proportional out to 25 mm, a full half corridor, and `kd` 0.008 -> 0.05 - damps what is left. Over a 120 s explore: lateral rms 25.0 -> 8.7 mm, first - ERROR 29.0 -> 98.2 s, goal reached at 38.8 s instead of never. `kd` above 0.05 - lets the derivative kick on the step the reading takes when a wall ends. - -**The remaining failure mode is the 180 deg spin.** `push_exploring` queues `stop` -(`cell_size / 2`), `turn_back`, `move_half`, and `turn_back` pivots about the body -origin with no safety margin. The chassis mesh puts the front tip at -`sqrt(15.5^2 + 66^2) = 67.8` mm from that origin and the corridor half-width is -83.7 mm, so the spin needs the body origin within **15.9 mm** of the cell centre. -It never is: `tools/turn_stats.py` shows every spin in every run starting 16 to -18 mm past the cell centre, because the `stop` action starts wherever the -accumulated longitudinal error left the robot and only measures 90 mm of travel -from there. Every spin therefore scrapes, and on maze 2 the spin at 25.7 s is the -first chassis contact of the run and the start of the 2.19 m odometry runaway that -kills it at 69.2 s. The spin is the least forgiving action in the repertoire and -the only one with no geometric margin; the exploring path has nothing equivalent -to the `curve_safety_margin` that `ActionQueuer::get_trim_distances` applies on -the solving path. - -Worth noting alongside that: the chassis mesh in `robot_v2.xml` is 66 mm wide and -99 mm long (-33..+66 in body y), while the sibling Micrasverse repo's `src/config/constants.hpp` -models the same robot as 50 x 80 mm. The mesh is 32 % wider and 24 % longer than -the other simulator's idea of the robot, and the wheels stick out a further 4 mm -each side. If the Micrasverse numbers are the measured robot then the mesh is -simply wrong, and correcting it would take the spin margin from 15.9 to about -25 mm. That is a measurement on the real chassis, not a simulation choice, so -nothing was changed here. +The `.clang-format`, `.clang-tidy`, `tests/.clang-tidy`, `cmake/micras_warnings.cmake` +and `cmake/templates/run_clang_tidy.sh.in` of micras-lib, copied byte for byte +(micras-lib holds the canonical copy and the firmware's CI compares the three +repositories), with clang 22's tools found by their versioned names; configuring +without them fails. The tidy header filter lints this repository's `include/` +headers and never a dependency's. Every target of the simulator gets the shared +warning list through `micras_apply_warnings`, and `MICRAS_SIM_WERROR` makes them errors +(the CI sets it); the dependencies are `SYSTEM`, so their headers raise nothing. +Containers are indexed with `at()`, and a span, which has no `at()` before C++26, +through `micras::sim::at` (`core/span_at.hpp`). `engine/src/.clang-tidy` tells +include-cleaner that toml++ is included through `toml.hpp`, and that a TOML +table's `operator[]` is a lookup, not an unchecked access. +Doxygen on every declaration. **No comments inside function bodies** โ€” if +something needs explaining, it goes in an `@note` on the declaration, where a +reader finds it before reading the code. Every `NOLINT` names its check and says +why. `micras_sim_lint` is clean and stays clean. ## Known gaps - **No "new run" from the panel.** It would need `RunControl::RESTART`, - restartable listeners (the recorder would have to reopen into `run_002/`), and - a fresh `FirmwareThread` and `Micras`, because the FSM, the maze map and the - in-memory `Storage` all carry state. That is an `Application` lifecycle change. -- **The video path is ungated.** `just check` proves a window and a bridge change - nothing, through `check-viewer` and `check-monitor`. There is no `check-video`, - and `just video` writes into `runs/explore_v2`, the very directory the baseline - comparison reads. Determinism under `--video` is believed, not tested. -- **`PlotTrace` drops its oldest sample with `erase(begin())`** rather than - being a real ring buffer. At 2048 samples and 30 fps it does not show, but it - is the wrong data structure. -- **Nine of the seventeen hardware tests are not built**: the human-interface - ones (`led`, `argb`, `buzzer`, `button`, `dip_switch`), plus `storage`, - `stopwatch`, `torque_sensors` and `comm_service`. `test_fan` needs `--button short`; the other seven run - unscripted. + restartable listeners, and a fresh `FirmwareThread` and firmware, because a + firmware's state machine, its map and its flash all carry state. +- **No minimum wall clearance in the baselines.** The event log has collisions, + but nothing measures the distance to the nearest wall yet. +- **No pinned container.** Byte identity is only checked between runs on one + machine. diff --git a/CMakeLists.txt b/CMakeLists.txt index 28f51d1..99e87e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,59 +1,87 @@ -cmake_minimum_required(VERSION 3.25) -project(micras_simulation LANGUAGES CXX) +cmake_minimum_required(VERSION 3.28...4.4) + +# Only a build of the simulator itself gets a default build type; a project that adds it keeps its own. +if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR AND NOT DEFINED CMAKE_BUILD_TYPE + AND NOT DEFINED ENV{CMAKE_BUILD_TYPE}) + set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Build type") +endif() -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +project(micras_simulation LANGUAGES CXX) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "Build type" FORCE) +if(PROJECT_IS_TOP_LEVEL) + set(CMAKE_CXX_STANDARD 23) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + set(CMAKE_CXX_SCAN_FOR_MODULES OFF) + set(CMAKE_EXPORT_COMPILE_COMMANDS ON) endif() -option(MICRAS_VIDEO "Build the offscreen EGL video recorder" ON) -option(MICRAS_VIEWER "Build the live GLFW viewer" ON) -option(MICRAS_BRIDGE "Build the WebSocket bridge to micras-monitor" ON) -option(MICRAS_TESTS "Build the unit tests" ON) +option(MICRAS_SIM_VIDEO "Build the offscreen EGL video recorder" ON) +option(MICRAS_SIM_VIEWER "Build the live GLFW viewer" ON) +option(MICRAS_SIM_BRIDGE "Build the WebSocket bridge to micras-monitor" ON) +option(MICRAS_SIM_TESTS "Build the unit tests" ${PROJECT_IS_TOP_LEVEL}) +option(MICRAS_SIM_TARGETS "Build the robot targets of targets/" ${PROJECT_IS_TOP_LEVEL}) +option(MICRAS_SIM_WERROR "Treat the simulator's compiler warnings as errors" OFF) +option(MICRAS_SIM_EXACT "Make the baseline checks also require byte identical runs" OFF) + +# A project that adds the simulator runs its tools and names its mazes. +set(MICRAS_SIM_TOOLS_DIR "${PROJECT_SOURCE_DIR}/tools" CACHE INTERNAL "The simulator's analysis and baseline tools") +set(MICRAS_SIM_MAZES_DIR "${PROJECT_SOURCE_DIR}/engine/arenas/maze/mazes" CACHE INTERNAL "The simulator's mazes") include(cmake/mujoco.cmake) include(cmake/third_party.cmake) +include(cmake/micras_warnings.cmake) ############################################################################### -## Simulation libraries +## Engine: knows no robot and no HAL ############################################################################### -add_library(micras_sim_core STATIC - src/sim/core/clock.cpp - src/sim/core/firmware_thread.cpp - src/sim/core/simulation.cpp - src/sim/core/mujoco_world.cpp - src/sim/core/serial_bus.cpp - src/sim/core/simulation_context.cpp +# The engine targets carry only their own include directories, so an engine source +# that includes a firmware or robot target header does not compile. + +add_library(micras_sim_engine STATIC + engine/arenas/maze/src/maze.cpp + engine/src/core/clock.cpp + engine/src/core/firmware_thread.cpp + engine/src/core/mujoco_world.cpp + engine/src/core/noise.cpp + engine/src/core/serial_bus.cpp + engine/src/core/simulation.cpp + engine/src/devices/dc_motor.cpp + engine/src/devices/digital_input.cpp + engine/src/devices/device.cpp + engine/src/devices/imu.cpp + engine/src/devices/power.cpp + engine/src/devices/quadrature_encoder.cpp + engine/src/devices/serial_link.cpp + engine/src/devices/wall_sensors.cpp + engine/src/recording/csv_recorder.cpp + engine/src/recording/csv_writer.cpp + engine/src/recording/event_log.cpp + engine/src/recording/ground_truth.cpp + engine/src/recording/run_metadata.cpp + engine/src/robot/robot_description.cpp + engine/src/robot/robot_model.cpp + engine/src/scenario/scenario.cpp ) -target_include_directories(micras_sim_core PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") -target_link_libraries(micras_sim_core PUBLIC mujoco::mujoco) - -include(cmake/firmware.cmake) - -add_library(micras_sim STATIC - src/sim/core/scenario.cpp - src/sim/telemetry/packet_framer.cpp - src/sim/telemetry/telemetry.cpp - src/sim/recording/csv_writer.cpp - src/sim/recording/ground_truth.cpp - src/sim/recording/csv_recorder.cpp - src/sim/recording/run_metadata.cpp +add_library(micras::sim_engine ALIAS micras_sim_engine) +target_include_directories(micras_sim_engine PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/engine/include" + "${CMAKE_CURRENT_SOURCE_DIR}/engine/arenas/maze/include" ) -target_link_libraries(micras_sim PUBLIC micras_sim_core micras_firmware_shadow PRIVATE picosha2) +target_link_libraries(micras_sim_engine PUBLIC mujoco::mujoco PRIVATE picosha2 tomlplusplus::tomlplusplus) add_library(micras_sim_view STATIC - src/sim/view/control_panel.cpp - src/sim/view/mujoco_viewer.cpp - src/sim/view/video_recorder.cpp + view/src/control_panel.cpp + view/src/mujoco_viewer.cpp + view/src/panel_spec.cpp + view/src/video_recorder.cpp ) -target_link_libraries(micras_sim_view PUBLIC micras_sim_core) +add_library(micras::sim_view ALIAS micras_sim_view) +target_include_directories(micras_sim_view PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/view/include") +target_link_libraries(micras_sim_view PUBLIC micras_sim_engine) -if(MICRAS_VIDEO) +if(MICRAS_SIM_VIDEO) set(OpenGL_GL_PREFERENCE GLVND) find_package(OpenGL COMPONENTS OpenGL EGL) @@ -63,10 +91,10 @@ if(MICRAS_VIDEO) target_link_libraries(micras_sim_view PRIVATE EGL GL) endif() - target_compile_definitions(micras_sim_view PRIVATE MICRAS_VIDEO) + target_compile_definitions(micras_sim_view PRIVATE MICRAS_SIM_VIDEO) endif() -if(MICRAS_VIEWER) +if(MICRAS_SIM_VIEWER) find_package(glfw3 QUIET) if(glfw3_FOUND) @@ -76,92 +104,97 @@ if(MICRAS_VIEWER) target_link_libraries(micras_sim_view PRIVATE "${GLFW_LIBRARY}") endif() - target_link_libraries(micras_sim_view PRIVATE imgui) - target_compile_definitions(micras_sim_view PRIVATE MICRAS_VIEWER) + target_link_libraries(micras_sim_view PRIVATE micras_sim_imgui) + target_compile_definitions(micras_sim_view PRIVATE MICRAS_SIM_VIEWER) endif() add_library(micras_sim_bridge STATIC - src/sim/bridge/monitor_bridge.cpp - src/sim/bridge/web_socket_server.cpp + bridge/src/monitor_bridge.cpp + bridge/src/web_socket_server.cpp ) -target_link_libraries(micras_sim_bridge PUBLIC micras_sim) +add_library(micras::sim_bridge ALIAS micras_sim_bridge) +target_include_directories(micras_sim_bridge PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/bridge/include") +target_link_libraries(micras_sim_bridge PUBLIC micras_sim_engine) -if(MICRAS_BRIDGE) +if(MICRAS_SIM_BRIDGE) target_link_libraries(micras_sim_bridge PRIVATE ixwebsocket) - target_compile_definitions(micras_sim_bridge PRIVATE MICRAS_BRIDGE) + target_compile_definitions(micras_sim_bridge PRIVATE MICRAS_SIM_BRIDGE) endif() -############################################################################### -## Executable -############################################################################### - -add_executable(micras_simulation src/main.cpp) -target_link_libraries(micras_simulation PRIVATE micras_sim_app) - # The application is the only layer that knows a run can have a window, so it is -# the only one that links the view. +# the only one that links the view. It has no main: each robot target calls +# micras::sim::run() from its own. add_library(micras_sim_app STATIC - src/sim/app/application.cpp - src/sim/app/cli.cpp - src/sim/app/crash_reporter.cpp + app/src/application.cpp + app/src/cli.cpp + app/src/crash_reporter.cpp ) -target_link_libraries(micras_sim_app PUBLIC micras_sim micras_sim_view micras_sim_bridge) - +add_library(micras::sim_app ALIAS micras_sim_app) +target_include_directories(micras_sim_app PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/app/include") +target_link_libraries(micras_sim_app PUBLIC micras_sim_engine micras_sim_view micras_sim_bridge) target_compile_definitions(micras_sim_app PRIVATE - MICRAS_FIRMWARE_SHA="${MICRAS_FIRMWARE_SHA}" + MICRAS_SIM_MAZES_DIR="${MICRAS_SIM_MAZES_DIR}" MICRAS_COMPILER_ID="${CMAKE_CXX_COMPILER_ID}" MICRAS_COMPILER_VERSION="${CMAKE_CXX_COMPILER_VERSION}" MICRAS_BUILD_TYPE="${CMAKE_BUILD_TYPE}" ) -set_target_properties(micras_simulation PROPERTIES - BUILD_RPATH "${MUJOCO_DIR}/lib" - INSTALL_RPATH "${MUJOCO_DIR}/lib" -) - -############################################################################### -## Firmware hardware tests -############################################################################### +# The public headers need C++23, so a project that adds the simulator gets it with them. +foreach(TARGET micras_sim_engine micras_sim_view micras_sim_bridge micras_sim_app) + target_compile_features(${TARGET} PUBLIC cxx_std_23) + set_target_properties(${TARGET} PROPERTIES CXX_EXTENSIONS OFF) +endforeach() -include(cmake/hardware_tests.cmake) - -micras_add_hardware_tests( - tests/src/proxy/test_battery.cpp - tests/src/proxy/test_fan.cpp - tests/src/proxy/test_imu.cpp - tests/src/proxy/test_locomotion.cpp - tests/src/proxy/test_rotary_sensors.cpp - tests/src/proxy/test_wall_sensors.cpp - tests/src/nav/test_odometry.cpp - tests/src/nav/test_calibrate_feed_forward.cpp -) +micras_apply_warnings(micras_sim_engine micras_sim_view micras_sim_bridge micras_sim_app WERROR ${MICRAS_SIM_WERROR}) ############################################################################### -## Tests +## Tests and robot targets ############################################################################### -if(MICRAS_TESTS) +if(MICRAS_SIM_TESTS) enable_testing() add_subdirectory(tests) endif() -############################################################################### -## Format and lint -############################################################################### +# Every robot target is a folder of targets/ with its own CMakeLists.txt. +if(MICRAS_SIM_TARGETS) + file(GLOB MICRAS_SIM_TARGET_DIRS LIST_DIRECTORIES true CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/targets/*") -file(GLOB_RECURSE HARNESS_SOURCES CONFIGURE_DEPENDS - "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tests/*.cpp" -) -file(GLOB_RECURSE HARNESS_HEADERS CONFIGURE_DEPENDS - "${CMAKE_CURRENT_SOURCE_DIR}/config/*.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/include/*.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tests/*.hpp" -) + foreach(target_dir IN LISTS MICRAS_SIM_TARGET_DIRS) + if(EXISTS "${target_dir}/CMakeLists.txt") + add_subdirectory("${target_dir}") + endif() + endforeach() +endif() -foreach(TARGET micras_sim_core micras_sim micras_sim_app micras_sim_view micras_sim_bridge micras_simulation) - target_compile_options(${TARGET} PRIVATE -Wall -Wextra) -endforeach() +############################################################################### +## Format, lint and checks: the simulator's own, so a project that adds it keeps its own +############################################################################### -include(cmake/lint.cmake) -micras_add_lint_targets(${HARNESS_SOURCES} ${HARNESS_HEADERS}) +if(PROJECT_IS_TOP_LEVEL) + file(GLOB_RECURSE HARNESS_SOURCES CONFIGURE_DEPENDS + "${PROJECT_SOURCE_DIR}/engine/*.cpp" + "${PROJECT_SOURCE_DIR}/view/*.cpp" + "${PROJECT_SOURCE_DIR}/bridge/*.cpp" + "${PROJECT_SOURCE_DIR}/app/*.cpp" + "${PROJECT_SOURCE_DIR}/tests/*.cpp" + "${PROJECT_SOURCE_DIR}/targets/*/src/*.cpp" + "${PROJECT_SOURCE_DIR}/targets/*/cube/*.cpp" + "${PROJECT_SOURCE_DIR}/targets/*/tests/*.cpp" + "${PROJECT_SOURCE_DIR}/targets/*/tools/*.cpp" + ) + file(GLOB_RECURSE HARNESS_HEADERS CONFIGURE_DEPENDS + "${PROJECT_SOURCE_DIR}/engine/*.hpp" + "${PROJECT_SOURCE_DIR}/view/*.hpp" + "${PROJECT_SOURCE_DIR}/bridge/*.hpp" + "${PROJECT_SOURCE_DIR}/app/*.hpp" + "${PROJECT_SOURCE_DIR}/tests/*.hpp" + "${PROJECT_SOURCE_DIR}/targets/*/include/*.hpp" + "${PROJECT_SOURCE_DIR}/targets/*/tests/*.hpp" + "${PROJECT_SOURCE_DIR}/targets/*/cube/*.h" + ) + + include(cmake/lint.cmake) + micras_sim_add_lint_targets(FORMAT ${HARNESS_SOURCES} ${HARNESS_HEADERS} LINT ${HARNESS_SOURCES}) + include(cmake/checks.cmake) +endif() diff --git a/CMakePresets.json b/CMakePresets.json index c7ba6d5..14630f0 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -2,7 +2,7 @@ "version": 6, "cmakeMinimumRequired": { "major": 3, - "minor": 25, + "minor": 28, "patch": 0 }, "configurePresets": [ @@ -12,52 +12,72 @@ "generator": "Ninja", "binaryDir": "${sourceDir}/build/${presetName}", "cacheVariables": { + "CMAKE_C_COMPILER": "gcc-15", + "CMAKE_CXX_COMPILER": "g++-15", "CMAKE_EXPORT_COMPILE_COMMANDS": "ON" } }, { - "name": "default", - "displayName": "RelWithDebInfo", + "name": "host", + "displayName": "Debug, with the address and undefined behavior sanitizers", "inherits": "base", "cacheVariables": { - "CMAKE_BUILD_TYPE": "RelWithDebInfo" + "CMAKE_BUILD_TYPE": "Debug", + "CMAKE_C_FLAGS": "-fsanitize=address,undefined -fno-omit-frame-pointer", + "CMAKE_CXX_FLAGS": "-fsanitize=address,undefined -fno-omit-frame-pointer", + "CMAKE_EXE_LINKER_FLAGS": "-fsanitize=address,undefined", + "CMAKE_SHARED_LINKER_FLAGS": "-fsanitize=address,undefined" } }, { - "name": "debug", - "displayName": "Debug", + "name": "host-release", + "displayName": "RelWithDebInfo, for runs and baselines", "inherits": "base", "cacheVariables": { - "CMAKE_BUILD_TYPE": "Debug" + "CMAKE_BUILD_TYPE": "RelWithDebInfo" } }, { - "name": "release", - "displayName": "Release", - "inherits": "base", + "name": "host-ci", + "displayName": "The CI's: host, with warnings as errors", + "inherits": "host", "cacheVariables": { - "CMAKE_BUILD_TYPE": "Release" + "MICRAS_SIM_WERROR": "ON" } } ], "buildPresets": [ { - "name": "default", - "configurePreset": "default" + "name": "host", + "configurePreset": "host" }, { - "name": "debug", - "configurePreset": "debug" + "name": "host-release", + "configurePreset": "host-release" }, { - "name": "release", - "configurePreset": "release" + "name": "host-ci", + "configurePreset": "host-ci" } ], "testPresets": [ { - "name": "default", - "configurePreset": "default", + "name": "host", + "configurePreset": "host", + "output": { + "outputOnFailure": true + } + }, + { + "name": "host-release", + "configurePreset": "host-release", + "output": { + "outputOnFailure": true + } + }, + { + "name": "host-ci", + "configurePreset": "host-ci", "output": { "outputOnFailure": true } diff --git a/HANDOFF.md b/HANDOFF.md deleted file mode 100644 index d707853..0000000 --- a/HANDOFF.md +++ /dev/null @@ -1,442 +0,0 @@ -# Handoff - -For whoever picks this up next, human or agent. It says where the harness -stands, what the three rules are that must not be broken while changing it, and -what is queued, roughly in the order it is worth doing. - -Read `README.md` for how to use the thing and `CLAUDE.md` for why it is built -the way it is. This file only covers what is *left*. Two older documents carry -the reasoning behind most of it and are worth opening before doing any of the -physics or firmware work: `../HARNESS_PLAN.md` (the diagnosis, the 38 physics -experiments, the risks) and `../HARNESS_REDESIGN_PLAN.md` (the eleven slices -this harness was built in, plus the deviations recorded in its ยง8.1). - -## Where it stands - -The harness runs the Micras firmware unmodified against MuJoCo, deterministic -and headless for measuring, with an optional window, control panel, mp4 -recorder and a WebSocket bridge that micras-monitor connects to as if it were -the radio. `just check` is the gate and it is green: 80 unit tests, config drift -against the firmware, an all-options-off build, seven hardware-test smoke runs, -a window-versus-headless CSV comparison, a bridge round trip, three scenarios -analysed and asserted, and all three compared against three recorded baselines. -`just lint` is clean, zero diagnostics. - -Everything lives on branches, nothing is on `main`: - -- `Team-Micras/micras-simulation`, branch `feature/simulation-harness` -- `Team-Micras/MicrasFirmware`, branch `feature/sim-harness` (the submodule) - -To build, you need MuJoCo unpacked at `$HOME/.mujoco/mujoco-3.3.6` (or pass -`-DMUJOCO_DIR=`), then `just build`. Clone with `--recurse-submodules`; the -submodule gitlink already points at the firmware commits the harness needs. - -The documentation was fact-checked against the code and its findings applied. -The bridge was reviewed too, and it came back with a reproduced CRITICAL โ€” that -is job 0 below, and it should be fixed before anyone demos the monitor. - -## The three rules - -Break these and the harness stops being worth having. - -**1. A run is reproducible.** Two runs of the same binary with the same -arguments produce byte-identical `data.csv` and `meta.json`. Nothing may make -the firmware's view of the world depend on wall time, on a window being open, on -a client being connected, or on how fast the machine is. - -**2. A refactoring must not move a byte.** If `data.csv` stops matching -`baseline/v2` after a change that was supposed to be behaviour-neutral, the -change is wrong. Revert it. Do not re-record the baseline to make the diff go -away โ€” that is the one move that destroys the whole safety net, and it is -exactly the tempting one. - -**3. A change that legitimately alters physics or columns gets a NEW baseline -version, never an overwrite.** `just record-baseline` refuses to overwrite, on -purpose. Bump `baseline` in the `justfile` to `baseline/v3`, move the old one -into `previous_baselines`, and the older recordings stay comparable as a header -prefix. This is the correct path for job 2 below. - -Corollary of rule 3: new CSV columns go at the **very end**. The prefix check is -literally a header prefix, so appending a pool variable in the middle would -shift every `proxy_*` column right and break it. - -## Job 0 โ€” The bridge hangs the simulation, and two things around it lie - -An independent review of the bridge reproduced all of this, with a gdb -backtrace and with the real binary. Do this before the rest. - -**The hang (critical).** `WebSocketServer::broadcast` calls `sendBinary` -synchronously on the simulation thread (`src/sim/bridge/web_socket_server.cpp`, -from `MonitorBridge::on_after_tick`). IXWebSocket's `flushSendBuffer` loops on -`isReadyToWrite(10)` with **no deadline and no give-up**; the only thing that -would break it out is a `stop()` that the blocked thread itself would have to -call. So a monitor that stops draining โ€” a throttled browser tab, a paused -debugger, a stalled tunnel โ€” applies TCP backpressure and the run freezes -indefinitely. Reproduced: row count frozen for 15 s and 18 s across three runs -with the firmware thread parked in `futex_do_wait`, resuming only when the -client's connection actually died. The `FirmwareThread` watchdog does not cover -it, because the firmware thread is the one waiting. `just check-monitor` in CI -would hang forever rather than fail. - -This is not exotic. Measured throughput to a *reading* client is about -3.2 MiB/s, roughly 232 000 packets/s, because the harness runs about 12ร— real -time. Anything slower than that is backpressure. - -The fix is to get `sendBinary` off the simulation thread: hand `outgoing` to a -bounded ring drained by a dedicated sender thread, drop the oldest frames when -it is full, and count the drops. The monitor is a view, not a ledger. Coalescing -several ticks into one frame is worth doing at the same time โ€” at ~12 000 -frames/s the framing overhead is most of the traffic. See also job 5, which is -the same problem seen from the radio's side. - -**A monitor-driven run claims to be reproducible when it is not (high).** -`application.cpp` computes `interactive` from the viewer alone. `MonitorBridge` -queues whatever the monitor sends straight into the firmware and never sets -`driven_by_human`, so the scenario also keeps scripting alongside the human. -Reproduced: two 20 s runs, one with a monitor injecting a `SERIAL_VARIABLE` -packet, produced different CSVs and both reported `interactive: false`. A -perturbed run is currently indistinguishable from a clean one and would be -accepted by `just check` or recorded as a baseline. Set `driven_by_human` (or a -`monitor_input_seen` flag) the first time the bridge queues a packet and OR it -into `RunMetadata::interactive`. The plan requires exactly this. - -**The check that was supposed to catch that can pass while testing nothing -(high).** When port 8080 is taken the binary prints `monitor disabled: Address -already in use` and **exits 0**, so `check-monitor`'s `runs/monitor/quiet` is -just another bridge-less run and the comparison passes trivially. Reproduced. -Make the recipe fail when the bridge could not listen, and pick a per-invocation -port instead of the hardcoded default. - -**Dropped packets are invisible (high).** `SerialBus::queue_for_firmware` -silently discards everything past 256 queued packets and increments a counter -that **nothing ever reads**. Flooding the bridge with 100 000 valid packets let -256 through and ~99 744 vanished without a trace in `meta.json`, stderr or the -CSV. Surface `dropped_packets` and the bridge's own `resync_count` in the -metadata next to `telemetry_resyncs`. - -Smaller ones from the same review, worth folding in while you are there: neither -bridge class has any test; `server` should be declared **last** in -`MonitorBridge` so the language guarantees the destruction order the destructor -body currently guarantees by hand; the bridge should reject frames failing -`comm::Packet::is_valid` before queueing them, because a client can otherwise -occupy the one-packet-per-tick slot with garbage and delay the scenario's own -injection; and `--monitor`/`--monitor-port` are parsed inside -`apply_viewer_option`, which is the wrong place and makes that function's own -`@brief` false. - -What the review confirmed as solid, so you do not need to re-derive it: the wire -protocol matches the firmware exactly (checked three ways, including a payload -full of control bytes), determinism holds for a no-client run, a refused port -and a well-behaved client, hostile input neither crashes nor leaks across 18 MB -of adversarial traffic, and the server binds to loopback only. - -## Job 1 โ€” The robot still does not finish maze 2 - -This is the actual goal and it is not met. Everything else on this list is -infrastructure around it. - -Where it is: on `models/maze.txt` the explore reaches a goal cell at 38.8 s, -returns to `{0, 0}` and goes IDLE at 94.6 s โ€” the maze is solved. On -`models/maze2.txt`, the 2019 Portugal qualifier, it does not reach the goal and -dies at 69.2 s. So the tuning is not maze independent, and a robot that only -solves the maze it was tuned on has not been shown to work. - -The failure has been isolated and it is **geometric, not a control problem**. -`push_exploring` queues `stop`, `turn_back`, `move_half`, and `turn_back` pivots -about the body origin with no safety margin. The chassis mesh puts the front tip -at `sqrt(15.5ยฒ + 66ยฒ) = 67.8` mm from that origin against a corridor half-width -of 83.7 mm, so the spin needs the body origin within **15.9 mm** of the cell -centre, and `tools/turn_stats.py` shows every spin in every run starting 16 to -18 mm past it โ€” because `stop` starts wherever the accumulated longitudinal -error left the robot and only measures 90 mm of travel from there. Every spin -scrapes. On maze 2 the spin at 25.7 s is the first chassis contact of the run -and the start of the 2.19 m odometry runaway that kills it. - -Three things it depends on, and only one of them is code: - -1. **Measure the real chassis.** The mesh in `robot_v2.xml` is 66 mm wide and - 99 mm long; the sibling Micrasverse repo models the same robot as 50 ร— 80 mm. - That is 32 % wider and 24 % longer, and the wheels stick out another 4 mm - each side. If Micrasverse is right, the mesh is simply wrong and fixing it - takes the spin margin from 15.9 to about 25 mm โ€” which may be the whole bug. - **Somebody has to put a caliper on the real robot.** Nobody should touch the - mesh before that number exists. -2. **Give `turn_back` a safety margin on the exploring path.** The solving path - already has one โ€” `ActionQueuer::get_trim_distances` applies - `curve_safety_margin` to the 180ยฐ case. The exploring path has nothing - equivalent. This is firmware nav work. -3. **`post_reference` is wrong, not `post_threshold`.** Lowering the threshold - was swept over 400/100/50/30/20 and is a clear negative โ€” the correction - fires constantly and injects error. Fixing it needs a measurement of where - the simulated post edge actually falls, which nobody has taken. - -**A firmware bug found during that work and never fixed**, still present at -`MicrasFirmware/micras_nav/src/action_queuer.cpp:83-88`: the `turned_left` -branch of `push_exploring` queues `move_to_turn, turn_left, move_to_turn`, while -`turned_right` two blocks below queues `move_to_turn, turn_right, -move_from_turn`. Almost certainly copy-paste โ€” the left turn exits on the -wrong action, and with no `follow_wall` where the right turn has one. Fix it -under rule 3 (it changes behaviour, so it needs a new baseline), and check -whether it moves the maze 2 result before doing anything else on this list. - -Also open, from the same investigation: **23 differences between -`config/constants.hpp` and the firmware are still marked `TODO`** in the -allowlist at the top of `tools/check_config_drift.py`, meaning allowlisted but -never explained. `just drift` prints the count. The most suspicious is the sign -of `right_feed_forward.angular_acceleration` (+0.1 in the harness against --0.0244 in the firmware, while the left one keeps its sign). - -## Job 2 โ€” MuJoCo 3.3.6 to 3.13.0 - -Worth doing and nothing structural is in the way. 3.13.0 is real and current -(released 2026-09-09); the pin at 3.3.6 is just where the work started. - -MuJoCo is located in exactly one place, `cmake/mujoco.cmake`, which points -`MUJOCO_DIR` at `$HOME/.mujoco/mujoco-3.3.6` and imports the shared library by -hand because the tarball ships no CMake config. `application.cpp` checks -`mj_version() != mjVERSION_HEADER` at startup, so a library/header mismatch is -caught loudly rather than as corruption. - -Expect this to change the numbers. Solver, contact and integrator work landed -across ten minor releases; `data.csv` will almost certainly differ. **That is -fine and it is rule 3, not rule 2.** The procedure: - -1. Unpack 3.13.0 next to the old one and bump the default in - `cmake/mujoco.cmake`. Keep the old directory around until the end. -2. `just build`. Fix whatever the API moved. Compile errors will concentrate in - `src/sim/core/mujoco_world.cpp`, `src/sim/recording/ground_truth.cpp` (it - touches `mjData` contact arrays directly) and `src/sim/view/*` (`mjv_`/`mjr_` - are the most churn-prone surface). -3. `just test` and `just lint` first โ€” those must pass unchanged, because - nothing in them depends on physics. -4. `just check` will now fail at `compare-baseline`. Before accepting that, - confirm the difference is *physics* and not a bug you introduced: run - `just analyze` on the three scenarios and compare `report.json` against the - old one. Warnings must still be zero, `telemetry_resyncs` zero, no - non-finite samples, and the explore run must still behave qualitatively the - same. A solver change moves trajectories a little; a mistake usually shows up - as warnings, NaNs, or the robot falling through the floor. -5. Only then bump to `baseline/v3` and `just record-baseline`. Keep v2, v1 and - v0 in `previous_baselines`. -6. Re-check the tuning claims in `CLAUDE.md`'s "State of the tuning" section. - Those numbers (goal at 38.8 s, first ERROR at 98.2 s, lateral rms 8.7 mm) - were measured under 3.3.6 and will move. Restate them or mark them as - measured under the old version โ€” do not leave them silently wrong. - -`mujoco_version` is recorded in `meta.json` but is deliberately not one of -`compare_run.py`'s `BEHAVIOUR_FIELDS`, so the provenance is kept without the -version itself failing a comparison. - -## Job 3 โ€” Get Micras out of the command line - -The complaint is fair: `Cli` knows far too much about this specific robot. -`--command none|explore|solve|calibrate` hardcodes four firmware pool variable -names; `--dip fan=0,diagonal=0,boost=0,risky=0` hardcodes the four switch -meanings in a `std::array` in `src/sim/app/cli.cpp`; `--no-fan` is a flag for -one actuator. Every one of those is a Micras fact compiled into the harness. - -What they all actually are: a script of timed writes to the board interface, -plus one timed write to a pool variable. That is the general shape, and it -belongs in **data**, not in C++: - -- a run script (a small file, or a repeatable `--at ` flag) with two - primitives: set pool variable `` to `` at time `t`, and hold the - button for `` starting at time `t`, plus an initial switch word; -- `ScenarioScript`'s `Command` enum and the `dip_names` array deleted, replaced - by names carried in the script; -- the current flags kept as thin sugar over it, or dropped, but only after the - scripts that use them are updated โ€” `tools/` and the `justfile` depend on the - current spelling of `--command explore`. - -Do it under rule 2: an identical scenario expressed the new way must produce a -byte-identical `data.csv`. That is a strong test and it is cheap to run. - -## Job 4 โ€” Get Micras out of the CSV recorder - -Same complaint, bigger blast radius, and the reason to do it after job 3. - -`src/sim/recording/ground_truth.cpp` is where it concentrates: wheel joint -names, the left/right wheel geoms, the caster, the `base` geom, four wall -sensors, the body forward axis being +y. A different robot, or the same robot -with a renamed geom, needs a C++ edit today. - -The shape to aim for is a column source interface โ€” something with `headers()` -and `values()` โ€” where `GroundTruth` is *built from a description* (which bodies, -which geoms, which sensors) rather than from constants, and the description -lives with the model. `CsvRecorder` already composes three sources (ground -truth, `ProxyState`, the firmware pool), so the seam exists; it is `GroundTruth` -itself that is hardcoded. - -Two things to preserve while doing it, both non-obvious and both load-bearing: - -- the exact column order and the `%.9g` formatting in `CsvWriter`, or rule 2 - fails immediately; -- the `nan` versus `0` distinction in `left_penetration`/`right_penetration`. - `nan` means "no contact at all" and `0` means "touching exactly". Collapsing - them loses real information that `analyze.py` reads. - -## Job 5 โ€” The radio is lying about its bandwidth - -This one is not cosmetic and it is the one most likely to cost a race. - -Measured on this branch: the firmware pushes **290 bytes per tick** through the -serial proxy, which at the 959.7 Hz loop is **about 278 kB/s, roughly 2.8 Mbaud -at 8N1**. A classic SPP link at 115200 baud carries 11.5 kB/s. The harness is -running about **24 times over** what the real radio can sustain, and nothing -anywhere notices. - -So the simulated robot reports state the real robot physically cannot send. Any -conclusion about telemetry drawn in simulation is optimistic by more than an -order of magnitude, and the firmware's own `enable_mask` โ€” which exists exactly -to ration this โ€” is never under pressure here, so it is never exercised. - -Two ways to fix it, and they are not equivalent: - -- **Cheap and non-breaking:** count the bytes and report the achieved rate in - `meta.json`, plus a threshold that flags a run exceeding a configured link - budget. New metadata field only, so `data.csv` does not move and rule 2 holds. - Do this first โ€” it makes the problem visible immediately. -- **Honest and behaviour-changing:** give `SerialBus` a byte budget per - simulated second and make it actually back up when the firmware exceeds it, - the way a real UART does. This changes what the firmware sees, so it is a new - baseline under rule 3, and it will probably expose firmware behaviour nobody - has looked at yet. That is the point of it. - -The budget should be configurable, because the answer depends on which radio the -robot actually carries. - -**This was already queued on the firmware side**, with options chosen, and the -harness now gives you the measurement to justify picking one. The stream needed -about 1.6 Mbaud before the new monitoring variables and about 2.5 Mbaud after. -The options on the table: decimate `send_serial_variables` (1 in 10 gives about -96 Hz, which is plenty for a human watching); remove the redundancy, since -`Odometry State` duplicates velocities already sent separately; and -`send_data(std::span)` plus a ring buffer to get the heap out of the control -loop. - -And a real firmware bug sitting underneath all of it: -`BluetoothSerial::trim_tx_queue` (`micras_proxy/src/bluetooth_serial.cpp:63`) -erases **raw bytes** from the front of the queue when it overflows. It cuts -wherever it lands, mid-packet, so the packet straddling the cut is corrupted and -the receiver resyncs. Whatever else is done about bandwidth, dropping should be -per whole packet โ€” drop-newest or drop-oldest, but never half of one. - -## Job 6 โ€” The singleton, and the decision that actually removes it - -`sim::SimulationContext::instance()` is a function-local static. It is not there -because anyone liked it; `CLAUDE.md`'s shadow-proxy section and Appendix A of -`HARNESS_REDESIGN_PLAN.md` have the full argument, but the short version is that -the firmware builds its proxies as member initialisers from `const` globals in -`target.hpp`, so at construction time the only route to the world is something -reachable from a `Config`. **A static class would have the same property and the -same problem** โ€” the issue is process-wide reachability, not the spelling. - -It is already fenced: exactly three places name `instance()` โ€” `target.hpp`, -`Application`'s member initialiser, and the context's own file. Everything else -receives facets through its `Config`, and the tests build their own context, -which is what makes the fence real rather than aspirational. - -**The decision that actually removes it has already been taken** (2026-09-21, -priority "soon", recorded at the end of `../HARNESS_PLAN.md` ยง7): `Micras` takes -its proxies through its constructor as `shared_ptr`, exactly as `Odometry`, -`FollowWall` and `SpeedController` already do, and the hardware `main.cpp` -assembles the real proxies from `target.hpp`. That single change kills the -header-shadowing mechanism, the singleton, **and** the three divergent copies of -the proxy layer that `tools/check_config_drift.py` exists to police. It needs an -interface or a C++20 concept per proxy so the simulated and the real one are -interchangeable, and it wants a firmware CMake that can build -`micras::core/nav/comm` without CubeMX, with something like -`MICRAS_PROXY_IMPL=`. - -Until that lands, tightening the fence is worth more than replacing the -mechanism. Do not invent a third way around it. - -## Firmware backlog, already diagnosed - -Small, understood, and none of it done. Each needs a new baseline under rule 3 -because each changes behaviour. - -- **`Interface` latches the command triggers.** `comm_explore`, `comm_solve` and - `comm_calibrate` are triggers, not switches, but they are never cleared after - generating their event, so the `true` stays latched and re-fires the moment the - FSM comes back to `IDLE`. This is why a run that finishes and goes IDLE - immediately starts another one. -- **`SERIAL_VARIABLE_CONTROL` has no bounds check.** A malformed id from the - radio indexes out of range. -- **The receive deque is cleared after framing a single packet**, which is why - the harness may never inject more than one packet per tick. It is a firmware - behaviour worth revisiting, not a harness limitation. -- **The rangefinder sentinel.** MuJoCo returns -1 for no hit, and the - `1 - exp(-c/xยฒ)` conversion cannot tell -1 from +1. Handle the sentinel - explicitly rather than letting it alias onto a real reading. -- **`solving.max_linear_acceleration = 12` is unreachable in the model.** With - the centre of mass about 20 mm ahead of the axle and a third of the weight on - the caster, maximum acceleration is around 6 m/sยฒ. When the motor model was - corrected this started showing up as a new "failure"; it is a finding, not a - regression. - -## What the plan asked for and is not there - -`../HARNESS_REDESIGN_PLAN.md` ยง2 is the list of decisions taken with the user -before any of this was built. Its ยง8.1 records the deviations that were made -deliberately during execution, with reasons โ€” read that before assuming -something was forgotten. These four, though, were agreed and simply are not -done: - -- **Screenshot from the viewer.** ยง2 lists it in the same breath as the render - flags ("flags de render, screenshot"). There is no key for it and no code. - `mjr_readPixels` is already used by `VideoRecorder`, so this is small. -- **Battery on the panel.** ยง2's minimum panel content includes it. It is not - there, and it cannot be until something feeds it: `ProxyState` has no battery - field, and the CSV column was removed once measurement showed this firmware - build never reads the battery at all (recorded in ยง8.1). So the honest version - of this item is a question โ€” should the panel show a value the firmware - ignores? โ€” not a coding task. -- **A bipolar fan control.** ยง2 asks for one. The panel has a "fan allowed" - checkbox, which is an override, not a control. -- **Reset in the window means a new run** (`run_002/`), so that every CSV is a - complete run. Deferred with reasons in ยง8.1; see the entry below. - -Also worth knowing when reading ยง2 against the code: it says -`SimulationContext::instance()` is used *only* by `target.hpp`. In practice -`Application`'s member initialiser names it too, which is where the process's -one context is picked up. That is correct and intended; the plan's sentence is -just narrower than reality. `CLAUDE.md` states the actual rule. - -## Also queued, smaller - -- **The video path is ungated.** `just check` proves a window and a bridge change - nothing; there is no `check-video`. Worse, `just video` writes into - `runs/explore_v2`, the directory the baseline comparison reads. Adding a - `check-video` step is a small job and closes a real hole. -- **No "new run" from the panel.** It needs `RunControl::RESTART`, restartable - listeners, and a fresh `FirmwareThread` and `Micras` โ€” the FSM, the maze map - and the in-memory `Storage` all carry state. It is an `Application` lifecycle - change, not panel work. -- **`PlotTrace` is not a ring buffer.** It drops its oldest sample with - `erase(begin())`. Invisible at 2048 samples and 30 fps, still the wrong data - structure. -- **Nine of the seventeen firmware hardware tests are not built** โ€” the - human-interface ones plus `storage`, `stopwatch`, `torque_sensors` and - `comm_service`. - -## Traps - -Short list of things that have already cost a debugging session here. - -- **EGL and GLX cannot both be current on one thread.** `--viewer --video` - requires `VideoRecorder` to release its EGL context after every capture and - `MujocoViewer::draw` to make the GLFW context current. Break the pair and you - get `BadAccess X_GLXMakeCurrent` and a zero-byte CSV. -- **The panel must use ImGui edge detection** (`IsItemActivated` / - `IsItemDeactivated`) for the board button. Writing it every frame from - `IsItemActive()` overwrites the scripted press and silently turns an - `extra_long` press into a short one. -- **The `Stopwatch` spin counters must survive a reset.** `Fan::update` resets - its own stopwatch every call, so a counter that reset with it would never - reach its threshold and `test_fan` would spin forever. -- **Never inject more than one serial packet per tick.** The firmware clears its - whole receive deque after framing one packet. -- **Use `xvfb-run -a` for anything with a window.** Running window tests on a - live display fights whoever is using the machine, and it has already killed a - test run here. diff --git a/MicrasFirmware b/MicrasFirmware deleted file mode 160000 index 9136d7b..0000000 --- a/MicrasFirmware +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9136d7b061756b0c02fdbd37437a960fd443c662 diff --git a/README.md b/README.md index 43b27a4..a29765d 100644 --- a/README.md +++ b/README.md @@ -1,111 +1,175 @@ # micras-simulation -Runs the Micras firmware, unmodified, against a MuJoCo model of the robot and -the maze. The firmware sees a proxy layer backed by physics instead of by -hardware, so the same `micras::Micras` that flies on the board drives a -simulated one here. +A simulator for the robots built on `micras_hal`. It runs a robot's firmware as +it is, its own `main`, its own proxies and its own configuration, over the host +backend of `micras_hal`, against a MuJoCo model of the robot and its arena. The +host backend and the models of the SPI chips live in +[micras-lib](https://github.com/Team-Micras/micras-lib), next to `micras_hal` +itself; a robot's project adds both. The micromouse, Micras, is the first robot +built on it: its firmware adds this repository as a submodule and holds its +target, its scenarios and its baselines in its own `sim/` folder, whose README +covers them. Two ways to use it, both from the same binary: - **Headless and deterministic**, for measuring. Every run writes a CSV of the - whole state per tick plus a metadata file, and two runs of the same binary - with the same arguments produce byte-identical output. + whole state plus a metadata file, and two runs of the same binary with the + same arguments produce byte-identical output. - **Live**, for looking. A window with the robot, a control panel with the board's buttons and switches, plots of the firmware's own variables, and a WebSocket bridge that micras-monitor connects to as if it were the radio. -Linux only. MuJoCo 3.3.6 is expected at `$HOME/.mujoco/mujoco-3.3.6`; override -with `-DMUJOCO_DIR=`. +Linux only, x86-64. CMake downloads the pinned MuJoCo release (3.14.0) and +checks its SHA-256; point `MUJOCO_DIR` (a CMake cache variable or an environment +variable) at an installation to use that instead. The style targets want clang +22's `clang-format-22`, `clang-tidy-22`, `run-clang-tidy-22` and +`clang-apply-replacements-22`. ## Quick start ```bash -just build -just run-explore # 8 s explore run -> runs/explore_v2 -just analyze runs/explore_v2 # report.json and the plots -just watch # the same scenario, 30 s, in a window -just serve # the same scenario, 120 s, open to micras-monitor -just check # the gate: everything above, with assertions +cmake --preset host-release +cmake --build --preset host-release +./build/host-release/targets/toy/toy_sim --scenario targets/toy/scenarios/drive.toml --out runs/drive +python3 tools/analyze.py runs/drive # report.json and the plots +./build/host-release/targets/toy/toy_sim --scenario targets/toy/scenarios/drive.toml --out runs/window --viewer +cmake --build --preset host --target micras_sim_check # the simulator's gate ``` -`just` with no arguments lists every recipe. The build uses `CMakePresets.json`; -`MICRAS_PRESET` picks between `default` (RelWithDebInfo), `debug` and `release`. -Only `default` has a test preset, so `just test` and `just check` need it. +Everything is CMake: the presets configure, and the checks are targets over the +bash scripts of `scripts/`; `cmake --build --preset --target help` lists +them. A run calls the binary directly (see [Running](#running)). + +| Preset | Build | For | +|---|---|---| +| `host` | Debug, with the address and undefined behavior sanitizers | the unit tests and the checks | +| `host-release` | RelWithDebInfo | runs and baselines | +| `host-ci` | `host`, with `MICRAS_SIM_WERROR=ON` | the CI | + +Each builds in `build//`, with GCC 15 (`gcc-15`/`g++-15`) and Ninja, since +another compiler may move the last bits of a run. ## Running ```bash -./build/default/micras_simulation --model models/robot_v2.xml --seconds 10 --out runs/t1 +./build/host-release/targets/toy/toy_sim --scenario targets/toy/scenarios/drive.toml --out runs/toy ``` | Flag | Meaning | |---|---| -| `--model ` | required, robot model | | `--out ` | required, output directory, created if missing | -| `--seconds ` | simulated seconds (default 10) | -| `--ticks ` | exact firmware tick count, overrides `--seconds` | -| `--maze ` | recorded in `meta.json` only; the model already attaches its maze | -| `--command none\|explore\|solve\|calibrate` | injects one `SERIAL_VARIABLE` packet writing `true` to the pool variable of the same name | -| `--command-at ` | when to inject it (default 0.5); never before tick 1, because tick 0 carries the map request | -| `--dip fan=0,diagonal=0,boost=0,risky=0` | DIP switch state before the first tick; indices follow `Interface::DipSwitchPins` | -| `--button none\|short\|long\|extra_long` | start through the `Button` proxy instead of a packet | -| `--button-at ` | when the button goes down (default 0.5) | -| `--no-fan` | the `Fan` proxy writes 0 to its actuator instead of the requested speed | +| `--scenario ` | the scenario to play; without one the robot is switched on and left alone | +| `--maze \|` | the arena: a maze of `engine/arenas/maze/mazes/` by name, or a file (default `maze1`) | +| `--seconds ` | simulated seconds, overriding the scenario's | +| `--ticks ` | exact tick count, overrides `--seconds` | +| `--seed ` | seed of every noise stream (default the scenario's, else 1) | +| `--ideal` | no noise at all | +| `--record-every ` | write one CSV row every N ticks (default 1); events are still detected every tick | | `--viewer` | open a window on the run | -| `--viewer-camera \|free` | starting camera (default `free`) | +| `--viewer-camera \|free` | starting camera | | `--viewer-fps ` | redraws per simulated second (default 30) | | `--viewer-size WxH` | window size, default 1200x900 | | `--monitor` | open the micras-monitor bridge | | `--monitor-port ` | port it listens on (default 8080) | | `--video ` | record an offscreen mp4 of the run | | `--video-fps ` | frame rate, default 30 | -| `--video-camera ` | `overhead`, `side tracking` (default), `robot pov`, `maze_top_view`, or `free` | +| `--video-camera ` | `side tracking` (the robot's default), `top tracking`, `onboard` (the robot's own view), `maze_top`, or `free` | | `--video-size WxH` | frame size, default 1280x720 | +| `--video-trail` | draw the path the robot has traveled behind it | + +A target adds options of its own (the toy's `--turn-angle`), which the usage +message lists. A window, a recording and a bridge are all optional and all off by default, and -none of them is meant to change what the run produces. `just check` proves that -for the window and for the bridge, by running the same scenario with and without -each and comparing the two CSVs byte for byte. The recording has no such gate -yet. +none of them changes what the run produces. `micras_sim_check` proves it for +each on the toy target, by running the same scenario with and without and +comparing the two runs byte for byte. + +## Scenarios + +```toml +# targets/toy/scenarios/drive.toml +robot = "toy" +arena = "maze1" +seconds = 10 +seed = 1 + +[start] +x = 0.09 +y = 0.05 +yaw_deg = 90.0 + +[[events]] +at = 0.2 +press = "button" +for = 0.1 + +[[events]] +at = 3.2 +send = "stop" + +[stop] +when = "state" +equals = ["STOP"] +after = 0.5 +``` + +An event can `press` an input for a time, `set` it, or `send` a link command +(the target names the inputs and the commands it knows). An event +with `when` and `equals` waits, from its `at` on, for the named firmware variable +to take one of those values, and fires on the first tick it does. The run stops +at `seconds`, or earlier when the stop condition's variable takes one of the +named values after `after` seconds, for the `count`-th time (default 1), or at +once when it takes one of the `abort` values. ## Output -`/data.csv` is one row per firmware tick: the ground truth from MuJoCo, -what crossed the proxy boundary in both directions, and every monitoring -variable the firmware publishes. +`/data.csv` is one row per tick (125 us) or per `--record-every` ticks: the +body pose and velocity, the robot's ground truth (wheels, motors, contacts, +solver), every monitoring variable the firmware publishes, and what each +simulated device produced. -`/meta.json` is the provenance of the run โ€” firmware SHA, model hash, -MuJoCo and compiler versions, the arguments, the tick and time counts, and the -assertions the gate checks (`warnings_total`, `pool_columns`, -`telemetry_resyncs`, `interactive`). Nothing measured off the wall clock goes in -it, so it is hash-stable too; the wall time is printed on stdout only. +`/meta.json` is the provenance and the outcome of the run: firmware commit, +hashes of `robot.toml` and of the composed model, MuJoCo and compiler versions, +the arguments, seed, tick counts, when the scenario stopped the run, the board's +counters (`unbound_ports`, `watchdog_expiries`, `emergency_stops`, +`serial_dropped_bytes`, `bridge_dropped_frames`), and the `events`: every state +change and collision, detected at full rate. Nothing measured off the wall clock +goes in it; the wall time is printed on stdout only. `interactive` is the honest flag: it is `true` only if a human actually touched -a control the firmware can see. A window that was merely open leaves it `false`, -and the run is still reproducible from `args`. +a control the firmware can see or a monitor sent it something. A window that was +merely open leaves it `false`, and the run is still reproducible from `args`. + +`/model.xml` is the MJCF the run simulated: the robot generated from its +`robot.toml`, composed with the arena. ## Analysis ```bash -just analyze runs/explore_v2 +python3 tools/analyze.py runs/drive ``` -Writes `report.json` and six plots into the run directory: speeds, contacts, -attitude, control, wheels, and a top-down trajectory coloured by time with the -goal cells shaded and the odometry estimate dashed. The report covers duration, -warnings, non-finite samples, per-wheel airborne fraction, slip, penetration, -chassis contact, control saturation, odometry error against ground truth, -deceleration events, the FSM transition timeline, and whether and when the goal -was reached. Only numpy and matplotlib are needed. - -`tools/run_stats.py` prints the per-run metrics, `tools/turn_stats.py` the -per-turn table (offset and heading error at entry, executed versus commanded -angle, and the smallest distance from the chassis to any wall during the turn). +Writes `report.json` and plots into the run directory: speeds, contacts, +attitude, actuators, wheels, a top-down trajectory over the maze with the goal +cells shaded and the firmware's pose estimate dashed, and whatever the target's +plugin adds. The +report covers warnings, non-finite samples, airborne fraction, slip, +penetration, chassis contact, the state timeline, collisions, whether and when +the goal was reached, the pose estimate's error against the ground truth, the +tracking error and voltage saturation. Only numpy and matplotlib are needed. + +`tools/analyze.py` knows only the engine's columns and needs only numpy and +matplotlib; what a robot's own columns mean comes from its plugin, `tools/analysis.py` in the target's folder, which +`meta.json` records as `target_dir`. `--plugin` or `$MICRAS_SIM_PLUGIN` name +another; `tools/baseline.py` finds it the same way. `baseline.py compare --exact` +is the byte identity check, and `tools/compare_run.py` compares two runs of one +build, ignoring the fields that name paths. ## Watching a run ```bash -just watch +./build/host-release/targets/toy/toy_sim --scenario targets/toy/scenarios/drive.toml --out runs/window --viewer ``` | Key | Action | @@ -117,74 +181,128 @@ just watch | esc | quit | Left drag orbits, right drag pans, middle drag and scroll zoom, and ctrl with -left drag pushes the robot around. The panel holds the board itself: the button, -the four DIP switches, the fan override, the LED, the two addressable LEDs and -the buzzer as the firmware is driving them, the wheel commands and wall -readings, and the desired versus measured linear and angular speed straight from -the firmware pool, one plot per variable. Pausing, stepping, a speed limiter and -a quit button are there too. - -Touching any board control hands the run over: the scripted `--command` and -`--button` stop being applied, and the run is marked `interactive`. +left drag pushes the robot around. The panel holds the board itself: the +button, the DIP switches, the lamps the firmware drives, the readouts and plots +of its variables, plus pause, step, a speed limiter and quit. Touching any board +control hands the run over: the scenario stops driving the inputs, and the run +is marked `interactive`. ## micras-monitor ```bash -just serve -``` - -The bridge puts the firmware's radio on `ws://localhost:8080`. It speaks the -same packet protocol the real board does, so micras-monitor connects to it -without knowing it is a simulation. One binary frame per tick carries everything -the firmware wrote; whatever the monitor sends is framed and queued on the bus, -which hands the firmware one packet per tick exactly as the radio would. - -`tools/monitor_probe.py` is a dependency-free client for checking the bridge -without the real monitor. - -## Firmware hardware tests - -The firmware's own `tests/` programs build against the same proxies: - -```bash -just run-test test_imu --seconds 4 --out runs/imu +./build/host-release/targets/toy/toy_sim --scenario targets/toy/scenarios/idle.toml --seconds 600 --out runs/link --monitor ``` -They take the same command line as the main program. `just smoke-tests` runs -each of them briefly, which is what catches a program that stops handing ticks -back to the simulation. +The bridge puts the firmware's radio on `ws://localhost:8080` and carries raw +bytes both ways, with no framing of its own: micras-monitor speaks the +firmware's protocol to it exactly as it would over the air. The link is as fast +as the radio's baud rate, not faster. ## The gate ```bash -just check +cmake --build --preset host --target micras_sim_check ``` -Builds, runs the unit tests through CTest, checks the harness config against the -firmware submodule for drift, configures every optional subsystem off to keep it -buildable, smoke-runs the hardware tests, proves that a window and a bridge -change nothing, runs the three scenarios, analyses them, asserts their metadata, -and compares all three against every recorded baseline. - -Baselines live in `baseline/`; `CLAUDE.md` explains the two invariants they -enforce. `just record-baseline` refuses to overwrite a version โ€” bump it -instead, and nothing is ever lost. - -`just format`, `just format-check` and `just lint` are the style targets; they -use the firmware's own `.clang-format` and `.clang-tidy`. +The simulator's gate, on the toy target (`targets/toy/`): builds everything, runs +every unit test through CTest, configures and builds every optional part off to +keep it buildable (`micras_sim_check_options`), checks that nothing outside +`targets/` names a robot (`micras_sim_check_generic`), and proves on the toy that +a run is reproducible and that a window, a video and a bridge nobody connects to +change nothing, and compares the toy's runs with its baseline +(`micras_sim_toy_check`). The window runs under `xvfb-run`, or on the display; +`MICRAS_SKIP_VIEWER=1` skips it, even where `xvfb-run` or a display exists. + +Each robot's baseline lives in `targets//baseline/`; `CLAUDE.md` explains +what it holds and the rules around it. `micras_sim_toy_record_baseline` records +the toy's over the previous one, which git keeps. With `-DMICRAS_SIM_EXACT=ON` a baseline +comparison also fails when a checked run's `data.csv` is not byte identical to the +recorded one: the check for a change that must not move a byte, such as a +refactoring. A project that adds the simulator reads the same option for its own +baselines. + +`micras_sim_format`, `micras_sim_format_check`, `micras_sim_lint` and +`micras_sim_lint_fix` are the style targets; they use the configuration shared +with micras-lib and the firmware (`.clang-format`, `.clang-tidy`). + +## A robot target + +A robot is a folder of `targets/` (or a directory of another project that adds +this one) with a class implementing `micras::sim::Target` and a `main` that is one +call to `micras::sim::run`. The target gives its name, loop period, robot file, +ground truth columns, options and firmware program, and `wire()` builds its +devices and returns what it adds to the run. The program is called with the +`FirmwareThread` it runs on, so a program that drives the handover itself calls +`yield_tick()` on it. + +The public headers, which a target may include and which change only with notice: + +| Header | What | +|---|---| +| `micras/sim/app/target.hpp` | `Target`, the interface | +| `micras/sim/app/wiring.hpp` | `Wiring`, `WorldInfo`, `CliOption` | +| `micras/sim/app/application.hpp` | `micras::sim::run` | +| `micras/sim/view/panel_spec.hpp` | the panel and the overlay | +| `micras/sim/core/run_context.hpp` | the world, the clock, the serial bus, the noise and the devices of a run | +| `micras/sim/core/firmware_thread.hpp` | the firmware thread and its handover | +| `micras/sim/core/variable_source.hpp` | the firmware variables the panel, the overlay and the scenarios read | +| `micras/sim/core/span_at.hpp` | checked indexing of a `std::span` | +| `micras/sim/recording/column_source.hpp`, `csv_writer.hpp` | CSV columns and the writer they fill | +| `micras/sim/recording/ground_truth.hpp`, `run_metadata.hpp` | ground truth, `meta.json` counters | +| `micras/sim/robot/robot_description.hpp` | the robot's `robot.toml` | +| `micras/sim/robot/robot_model.hpp` | `RobotModelNames`, the names the generated model gives the robot's parts | +| `micras/sim/scenario/scenario.hpp` | `ScenarioHooks`: the inputs, messages and state names a scenario uses | +| `micras/sim/devices/*.hpp` | the devices a target builds | + +A target over `micras_hal` links micras-lib's host backend and defines the board +target it asks for, the Cube layer its firmware includes, written by hand; +micras-lib's README describes that contract. A project that adds this repository +with `add_subdirectory` gets the libraries (`micras::sim_app` and the rest) and +`MICRAS_SIM_TOOLS_DIR` and `MICRAS_SIM_MAZES_DIR`, and keeps its own style and +check targets: the simulator defines its own only when it is the top-level +project. + +The robot is micromouse-shaped and runs in a maze: its `robot.toml` has every +section of the schema (chassis, wheels, drive, encoders, IMU, fan, battery, link, +wall sensors), and the arena is one of the mazes. + +## The toy target + +`targets/toy/` is the simulator's reference: the smallest robot that uses every +feature of the engine and of the `Target` interface, with no HAL and nothing of a +real robot. Its `robot.toml` is the engine tests' tiny robot with a second wall +sensor looking ahead. Its program, which yields on its own firmware thread once +per 1 ms loop, waits; a press of the button or `go` over the link starts it; it +drives with the fan on, counting its encoders, until the front wall sensor sees a +wall, turns right by `--turn-angle` degrees (90 by default) integrating its gyro, +drives on, and stops at `stop` over the link, which it answers. Its variables are +its state, odometry, heading, front reading, pack voltage and motor currents; its +panel has the button, a lamp per state and the pack voltage; its scenarios are +`idle` and `drive`. + +## CI + +`.github/workflows/ci.yaml` runs on every push and on pull requests from forks, +in the image of `.docker/Dockerfile` (Ubuntu 26.04 with GCC 15, clang 22, CMake +and Python; its host stage is the one micras-lib and the firmware use too): the +`host-ci` build, CTest, `micras_sim_format_check`, `micras_sim_lint` and +`micras_sim_check`. The baselines are compared within their tolerances there: a +hash belongs to the machine that recorded it. ## Layout ``` -config/ target.hpp (MuJoCo wiring) and constants.hpp (tuning) -include/micras/proxy/, src/proxy/ the 16 proxies the firmware sees -include/micras/sim/, src/sim/ core/, telemetry/, recording/, view/, bridge/, app/ -models/ robot and maze models, plus the tuning experiments -tools/ analysis, drift, assertions, comparison, monitor probe -tests/ unit tests; the hardware tests compile from MicrasFirmware/tests/src/ -baseline/ recorded runs the gate compares against -MicrasFirmware/ submodule +engine/ world, clock, run loop, devices, robot description, scenarios, recording +engine/arenas/ the maze arena and its mazes +view/ window, panel, overlay, video +bridge/ WebSocket server and monitor bridge +app/ CLI and application wiring, the Target interface +tests/ the engine's unit tests, on a tiny robot of their own +tools/ analysis, baselines, run health and byte comparison +scripts/ the checks' scripts, which the CMake targets call +cmake/ dependencies, warnings, style and check targets +targets/toy/ the reference target: toy robot, scenarios, baseline ``` -`CLAUDE.md` has the architecture, the invariants that hold the determinism -together, and the tuning history behind the numbers in `config/constants.hpp`. +`CLAUDE.md` has the architecture and the invariants that hold the determinism +together. diff --git a/app/include/micras/sim/app/application.hpp b/app/include/micras/sim/app/application.hpp new file mode 100644 index 0000000..b1691b7 --- /dev/null +++ b/app/include/micras/sim/app/application.hpp @@ -0,0 +1,178 @@ +/** + * @file + * + * @brief Wires a simulation together and runs one robot target in it. + */ + +#ifndef MICRAS_SIM_APP_APPLICATION_HPP +#define MICRAS_SIM_APP_APPLICATION_HPP + +#include +#include + +#include "micras/sim/app/cli.hpp" +#include "micras/sim/app/target.hpp" +#include "micras/sim/arenas/maze.hpp" +#include "micras/sim/bridge/monitor_bridge.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/devices/device_columns.hpp" +#include "micras/sim/recording/csv_recorder.hpp" +#include "micras/sim/recording/event_log.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/scenario/scenario.hpp" +#include "micras/sim/view/mujoco_viewer.hpp" +#include "micras/sim/view/video_recorder.hpp" + +namespace micras::sim { +/** + * @brief Builds the run around a robot target, then runs it. + * + * @note The only place that decides what a run is made of: it reads the + * scenario, builds the world from the robot's description and the arena, + * and registers every listener of one Simulation in the order Wiring + * documents. It is also the only layer that knows a run can have a window. + */ +class Application { +public: + /** + * @brief Set a simulation up for a parsed command line. + * + * @param options Parsed command line. + * @param target Robot to run. + */ + Application(const CliOptions& options, Target& target); + + Application(const Application&) = delete; + Application(Application&&) = delete; + Application& operator=(const Application&) = delete; + Application& operator=(Application&&) = delete; + + /** + * @brief Let the target undo what it pointed at the firmware thread. + */ + ~Application(); + + /** + * @brief Run the simulation and write the metadata. + * + * @param arguments Whole command line, recorded in the metadata. + */ + void run(std::span arguments); + +private: + /** + * @brief Parsed command line. + */ + CliOptions options; + + /** + * @brief Robot being run. + * + * @note Owned by the executable's main, which outlives the application. + */ + Target& target; // NOLINT(*-avoid-const-or-ref-data-members) + + /** + * @brief Path of the scenario file, empty for none. + */ + std::filesystem::path scenario_path; + + /** + * @brief Scenario being played. + */ + Scenario scenario; + + /** + * @brief The robot's physical description. + */ + RobotDescription robot; + + /** + * @brief Path of the maze drawing. + */ + std::filesystem::path maze_path; + + /** + * @brief Dimensions and surfaces of the maze. + */ + MazeConfig maze_config; + + /** + * @brief The target's context, with the world built and the clock configured. + * + * @note Bound for the life of the run; the target owns it. + */ + RunContext& context; // NOLINT(*-avoid-const-or-ref-data-members) + + /** + * @brief Number of ticks to run. + */ + uint64_t ticks; + + /** + * @brief The target's program, in its own thread. + */ + FirmwareThread firmware; + + /** + * @brief What the target adds to the run. + */ + Wiring wiring; + + /** + * @brief Plays the scenario. + */ + ScenarioPlayer player; + + /** + * @brief Notices collisions and state changes at full rate. + */ + EventLog events; + + /** + * @brief What the devices record, after the target's own columns. + */ + DeviceColumns device_columns; + + /** + * @brief CSV output. + */ + CsvRecorder recorder; + + /** + * @brief Offscreen video, or null. + */ + std::unique_ptr video; + + /** + * @brief Live window, or null. + */ + std::unique_ptr viewer; + + /** + * @brief Bridge to micras-monitor, or null. + */ + std::unique_ptr monitor; + + /** + * @brief The run itself. + */ + Simulation simulation; +}; + +/** + * @brief Run a robot target from its executable's main. + * + * @note Catches everything, prints it with the usage text and returns non-zero, + * so a failed run is loud and never looks successful. + * + * @param arguments Whole command line. + * @param target Robot to run. + * @return Process exit code. + */ +int run(std::span arguments, Target& target); +} // namespace micras::sim + +#endif // MICRAS_SIM_APP_APPLICATION_HPP diff --git a/include/micras/sim/app/cli.hpp b/app/include/micras/sim/app/cli.hpp similarity index 53% rename from include/micras/sim/app/cli.hpp rename to app/include/micras/sim/app/cli.hpp index 0da65e5..0a6d32a 100644 --- a/include/micras/sim/app/cli.hpp +++ b/app/include/micras/sim/app/cli.hpp @@ -9,10 +9,12 @@ #include #include +#include #include #include +#include -#include "micras/sim/core/scenario.hpp" +#include "micras/sim/app/wiring.hpp" #include "micras/sim/view/view_options.hpp" namespace micras::sim { @@ -21,38 +23,47 @@ namespace micras::sim { */ struct CliOptions { /** - * @brief Model to load and directory the run is written to. + * @brief Directory the run is written to. */ - ///@{ - std::filesystem::path model; std::filesystem::path out; - ///@} /** - * @brief Maze recorded in the metadata; the model already attaches its own. + * @brief Scenario file to play; empty for the robot's default. */ - std::string maze; + std::filesystem::path scenario; /** - * @brief Scripted input for the run. + * @brief Maze to run in, a name from the arena's collection or a path; overrides the scenario's. */ - ScenarioScript scenario; + std::string maze; /** - * @brief Length of the run, in simulated seconds or in exact ticks. + * @brief Length of the run, in simulated seconds or in exact ticks; overrides the scenario's. */ ///@{ - double seconds{10.0}; - uint64_t ticks{0}; + std::optional seconds; + std::optional ticks; ///@} /** - * @brief Offscreen recording, enabled by --video. + * @brief Seed of every noise stream; overrides the scenario's. + */ + std::optional seed; + + /** + * @brief Whether the world is ideal: no noise, no bias, no scale error. + */ + bool ideal{false}; + + /** + * @brief Ticks between two recorded rows. + */ + uint32_t record_every{1}; + + /** + * @brief Offscreen recording, enabled by --video, which names its file. */ - ///@{ VideoConfig video; - bool video_enabled{false}; - ///@} /** * @brief Live window, enabled by --viewer. @@ -76,7 +87,8 @@ struct CliOptions { * @brief Turns an argument vector into options, and nothing else. * * @note Pure: it never touches the filesystem, the model or the clock, so the - * whole command line surface is testable without a simulation. + * whole command line surface is testable without a simulation. The + * robot target's own options are handed to their handlers as they come. */ class Cli { public: @@ -84,17 +96,19 @@ class Cli { * @brief Parse a command line. * * @param arguments Whole command line, program name included. - * @param loop_time_us Firmware loop period, used to turn seconds into ticks. + * @param target_options Options the robot target adds. * @return Parsed options. */ - static CliOptions parse(std::span arguments, uint32_t loop_time_us); + static CliOptions parse(std::span arguments, std::span target_options = {}); /** * @brief Get the usage text. * + * @param program Name of the executable, as argv[0] gives it. + * @param target_options Options the robot target adds. * @return Usage text, newline terminated. */ - static std::string usage(); + static std::string usage(std::string_view program, std::span target_options = {}); }; } // namespace micras::sim diff --git a/include/micras/sim/app/crash_reporter.hpp b/app/include/micras/sim/app/crash_reporter.hpp similarity index 100% rename from include/micras/sim/app/crash_reporter.hpp rename to app/include/micras/sim/app/crash_reporter.hpp diff --git a/app/include/micras/sim/app/target.hpp b/app/include/micras/sim/app/target.hpp new file mode 100644 index 0000000..fd4a61c --- /dev/null +++ b/app/include/micras/sim/app/target.hpp @@ -0,0 +1,140 @@ +/** + * @file + * + * @brief What a robot target gives the application, and what it gets back. + */ + +#ifndef MICRAS_SIM_APP_TARGET_HPP +#define MICRAS_SIM_APP_TARGET_HPP + +#include +#include +#include +#include + +#include "micras/sim/app/wiring.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/recording/ground_truth.hpp" +#include "micras/sim/recording/run_metadata.hpp" + +namespace micras::sim { +/** + * @brief One robot, as the application sees it. + * + * @note Everything the engine must not know lives behind this interface: the + * firmware, its loop period, the model's names, the robot's own options, + * columns and panel. The application calls it in this order: options(), + * context(), program(), wire(), and after the run metadata() and + * unwire(). + */ +class Target { +public: + Target() = default; + + Target(const Target&) = delete; + Target(Target&&) = delete; + Target& operator=(const Target&) = delete; + Target& operator=(Target&&) = delete; + + virtual ~Target() = default; + + /** + * @brief Get the name of the robot, used for the window and meta.json. + * + * @return The name. + */ + virtual std::string name() const = 0; + + /** + * @brief Get the commit of the firmware this binary was built from. + * + * @return The commit, or "unknown". + */ + virtual std::string firmware_sha() const = 0; + + /** + * @brief Get the firmware loop period, which the physics timestep must divide. + * + * @return Period in microseconds. + */ + virtual uint32_t loop_time_us() const = 0; + + /** + * @brief Get the options this robot adds to the command line. + * + * @return The options; their handlers may capture the target. + */ + virtual std::vector options() { return {}; } + + /** + * @brief Get the context the run advances. + * + * @note The application builds the world in it and configures its clock. + * + * @return The context, which must outlive the run. + */ + virtual RunContext& context() = 0; + + /** + * @brief Get the folder of the target, whose tools/analysis.py is its analysis plugin. + * + * @return The folder, written to meta.json as target_dir. + */ + virtual std::filesystem::path directory() const = 0; + + /** + * @brief Get the robot's physical description. + * + * @return Path of its robot.toml, which the world is built from. + */ + virtual std::filesystem::path robot_file() const = 0; + + /** + * @brief Get the body and robot columns of the ground truth. + * + * @return The recorder configuration. + */ + virtual GroundTruthConfig ground_truth() const = 0; + + /** + * @brief Get the camera a video uses when the command line names none. + * + * @return A model camera name, or "free". + */ + virtual std::string video_camera() const { return "free"; } + + /** + * @brief Get the firmware program, run on the firmware thread. + * + * @return The program. + */ + virtual FirmwareThread::Program program() = 0; + + /** + * @brief Build what the robot adds to the run: its devices, columns and panel. + * + * @note Called once the world is built and the clock configured; the devices + * it adds to the context's run in the order added. + * + * @param firmware Thread that will run the program. + * @param world The robot the world was built from, and the arena's surfaces. + * @return Columns, variables, panel and scenario hooks of the robot. + */ + virtual Wiring wire(FirmwareThread& firmware, const WorldInfo& world) = 0; + + /** + * @brief Undo whatever wire() pointed at the firmware thread. + */ + virtual void unwire() { } + + /** + * @brief Get the robot's own counters for meta.json. + * + * @return The counters, in the order they are written. + */ + virtual std::vector metadata() const { return {}; } +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_APP_TARGET_HPP diff --git a/app/include/micras/sim/app/wiring.hpp b/app/include/micras/sim/app/wiring.hpp new file mode 100644 index 0000000..626c9a0 --- /dev/null +++ b/app/include/micras/sim/app/wiring.hpp @@ -0,0 +1,94 @@ +/** + * @file + * + * @brief What a robot target adds to a run: its options, its wiring, and what the world was built from. + */ + +#ifndef MICRAS_SIM_APP_WIRING_HPP +#define MICRAS_SIM_APP_WIRING_HPP + +#include +#include +#include + +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/recording/column_source.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/scenario/scenario.hpp" +#include "micras/sim/view/panel_spec.hpp" + +namespace micras::sim { +/** + * @brief A command line option a robot target adds. + */ +struct CliOption { + /** + * @brief Option as typed, for example "--button". + */ + std::string name; + + /** + * @brief What the value looks like in the usage text, empty for a flag. + */ + std::string argument; + + /** + * @brief Called with the value, or with an empty string for a flag. + * + * @note Throws std::runtime_error on a value it cannot accept. + */ + std::function apply; +}; + +/** + * @brief How a robot target plugs into one run. + * + * @note Listener order is part of what a run records, so the application fixes + * it: the crash reporter, the scenario, the monitor bridge, the event log, + * the recorder, the video, the window. The robot's devices run between + * the listeners, as the run loop says. + */ +struct Wiring { + /** + * @brief Column blocks written after the ground truth, in this order. + */ + std::vector columns; + + /** + * @brief Firmware variables the panel plots and the overlay prints, or null. + */ + const VariableSource* variables{nullptr}; + + /** + * @brief What the panel shows, which the application completes with the handover. + */ + PanelSpec panel; + + /** + * @brief What the video overlay prints. + */ + OverlaySpec overlay; + + /** + * @brief Inputs, link messages and state names scenarios act on. + */ + ScenarioHooks hooks; +}; + +/** + * @brief What the world was built from, for the devices a target builds. + */ +struct WorldInfo { + /** + * @brief The robot's physical description, which outlives the wiring. + */ + const RobotDescription* robot; + + /** + * @brief Infrared reflectance of an arena geom, by id. + */ + std::function reflectance; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_APP_WIRING_HPP diff --git a/app/src/application.cpp b/app/src/application.cpp new file mode 100644 index 0000000..fbd8d0a --- /dev/null +++ b/app/src/application.cpp @@ -0,0 +1,520 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "micras/sim/app/application.hpp" +#include "micras/sim/app/cli.hpp" +#include "micras/sim/app/crash_reporter.hpp" +#include "micras/sim/app/target.hpp" +#include "micras/sim/app/wiring.hpp" +#include "micras/sim/arenas/maze.hpp" +#include "micras/sim/bridge/monitor_bridge.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/recording/column_source.hpp" +#include "micras/sim/recording/ground_truth.hpp" +#include "micras/sim/recording/run_metadata.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/robot/robot_model.hpp" +#include "micras/sim/scenario/scenario.hpp" +#include "micras/sim/view/mujoco_viewer.hpp" +#include "micras/sim/view/panel_spec.hpp" +#include "micras/sim/view/video_recorder.hpp" +#include "micras/sim/view/view_options.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Duration of a run when neither the command line nor the scenario gives one. + */ +constexpr double default_seconds{10.0}; + +/** + * @brief Maze a run uses when neither the command line nor the scenario names one. + */ +constexpr std::string_view default_maze{"maze1"}; + +/** + * @brief Gap left between the robot's back and the wall behind it at the start, in meters. + */ +constexpr double start_gap{0.001}; +} // namespace + +/** + * @brief Find a maze drawing: a path, or a name from the arena's collection. + * + * @param name The path or name. + * @return The drawing's path. + */ +static std::filesystem::path resolve_maze(const std::string& name) { + std::filesystem::path given{name}; + + if (std::filesystem::exists(given)) { + return given; + } + + const std::filesystem::path collected = std::filesystem::path{MICRAS_SIM_MAZES_DIR} / (name + ".txt"); + + if (std::filesystem::exists(collected)) { + return collected; + } + + throw std::runtime_error("no maze named " + name + ": neither a file nor one of " + MICRAS_SIM_MAZES_DIR); +} + +/** + * @brief Choose the maze a run uses: the command line's, else the scenario's, else the default. + * + * @param options Parsed command line. + * @param scenario The scenario the run plays. + * @return The maze's path or name. + */ +static std::string maze_name(const CliOptions& options, const Scenario& scenario) { + if (not options.maze.empty()) { + return options.maze; + } + + if (not scenario.arena.empty()) { + return scenario.arena; + } + + return std::string{default_maze}; +} + +/** + * @brief Place a robot in the maze's start cell, its back against the wall behind it, facing up. + * + * @param maze The maze. + * @param config Its dimensions. + * @param robot The robot. + * @return The placement. + */ +static MujocoWorld::Placement start_of(const Maze& maze, const MazeConfig& config, const RobotDescription& robot) { + double rear = 0.0; + + for (const auto& point : robot.chassis.outline) { + rear = std::max(rear, -point.at(0)); + } + + const auto [column, row] = maze.start(); + + return { + .x = (static_cast(column) + 0.5) * config.cell_size, + .y = static_cast(row) * config.cell_size + config.wall_thickness / 2 + rear + start_gap, + .yaw = std::numbers::pi / 2, + }; +} + +/** + * @brief Read the scenario the run plays. + * + * @param options Parsed command line. + * @param target Robot to run. + * @param path Filled with the scenario's path, empty for none. + * @return The scenario, empty when there is none. + */ +static Scenario read_scenario(const CliOptions& options, const Target& target, std::filesystem::path& path) { + path = options.scenario; + + if (path.empty()) { + return {}; + } + + Scenario scenario = Scenario::load(path); + + if (not scenario.robot.empty() and scenario.robot != target.name()) { + throw std::runtime_error(path.string() + " is a scenario for " + scenario.robot + ", not " + target.name()); + } + + return scenario; +} + +/** + * @brief Build the world, configure the clock and the noise. + * + * @param robot The robot's description. + * @param maze_path Path of the maze drawing. + * @param config The maze's dimensions. + * @param options Parsed command line. + * @param scenario The scenario, which may place the robot. + * @param target Robot to run. + * @return The target's context. + */ +static RunContext& prepare( + const RobotDescription& robot, const std::filesystem::path& maze_path, const MazeConfig& config, + const CliOptions& options, const Scenario& scenario, Target& target +) { + RunContext& context = target.context(); + const Maze maze = Maze::load(maze_path); + + context.world.build( + robot_mjcf(robot), RobotModelNames::of(robot).body, maze.mjcf(config), Maze::body_name, + scenario.start.value_or(start_of(maze, config, robot)) + ); + + for (const SkidPart& skid : robot.chassis.skids) { + if (skid.friction > 0.0) { + context.world.add_sliding_friction(skid.name, skid.friction); + } + } + + context.clock = Clock::from_model(context.world.timestep(), target.loop_time_us()); + context.world.reset(); + context.noise = {.seed = options.seed.value_or(scenario.seed.value_or(1)), .ideal = options.ideal}; + return context; +} + +/** + * @brief Get how many ticks the run lasts. + * + * @param options Parsed command line. + * @param scenario The scenario. + * @param context The run's context, with its clock configured. + * @return Number of ticks. + */ +static uint64_t ticks_of(const CliOptions& options, const Scenario& scenario, const RunContext& context) { + if (options.ticks.has_value()) { + return *options.ticks; + } + + const double seconds = options.seconds.value_or(scenario.seconds > 0.0 ? scenario.seconds : default_seconds); + const uint64_t ticks = context.clock.total_ticks(seconds); + + if (ticks == 0) { + throw std::runtime_error(std::to_string(seconds) + " s is shorter than one firmware loop period"); + } + + return ticks; +} + +/** + * @brief Get how many ticks pass between two frames at a given rate. + * + * @param fps Frames per second of simulated time. + * @param clock Clock the frame period is derived from. + * @return Number of ticks per frame, at least one. + */ +static uint64_t ticks_per_frame(int fps, const Clock& clock) { + const double loop_period = clock.us_per_tick() * 1e-6; + return std::max(1, static_cast(std::llround(1.0 / (fps * loop_period)))); +} + +/** + * @brief Open the monitor bridge if the command line asked for it. + * + * @note A port that is already taken is reported and the run carries on + * without the bridge, rather than failing a run that did not need it. + * + * @param options Parsed command line. + * @param serial Bus the bridge attaches to. + * @return The bridge, or null when disabled or when the port was taken. + */ +static std::unique_ptr make_monitor(const CliOptions& options, SerialBus& serial) { + if (not options.monitor_enabled) { + return nullptr; + } + + std::string error; + auto bridge = std::make_unique(serial, options.monitor_port, error); + + if (not bridge->is_open()) { + std::cerr << "monitor disabled: " << error << '\n'; + return nullptr; + } + + std::cout << "monitor bridge listening on ws://localhost:" << options.monitor_port << '\n'; + return bridge; +} + +/** + * @brief Open the live window if the command line asked for it. + * + * @note The first board control a human touches hands the board over from the + * scenario, which then stops driving the inputs. + * + * @param options Parsed command line. + * @param context Context the window draws. + * @param target Robot whose name and body the window uses. + * @param wiring What the robot put on the panel. + * @param player The scenario, which gives the board up to the human. + * @return The viewer, or null when disabled or when no window could open. + */ +static std::unique_ptr make_viewer( + const CliOptions& options, RunContext& context, const Target& target, const Wiring& wiring, ScenarioPlayer& player +) { + if (not options.viewer_enabled) { + return nullptr; + } + + ViewerConfig config = options.viewer; + config.title = target.name(); + config.body = target.ground_truth().body; + config.ticks_per_frame = ticks_per_frame(options.viewer_fps, context.clock); + config.us_per_tick = context.clock.us_per_tick(); + + PanelSpec panel = wiring.panel; + panel.take_over = [&player] { player.hand_over(); }; + + std::string error; + auto viewer = MujocoViewer::create(context.world, std::move(panel), wiring.variables, config, error); + + if (viewer == nullptr) { + std::cerr << "viewer disabled: " << error << '\n'; + return nullptr; + } + + std::cout << "window open at " << config.width << "x" << config.height << ", camera '" << config.camera + << "', one frame every " << config.ticks_per_frame << " ticks\n"; + + return viewer; +} + +/** + * @brief Start the offscreen recorder if the command line asked for it. + * + * @param options Parsed command line. + * @param context Context the recorder draws. + * @param target Robot whose camera and body the recording uses. + * @param wiring What the robot prints on the overlay. + * @return The recorder, or null when disabled or when EGL is unavailable. + */ +static std::unique_ptr + make_video_recorder(const CliOptions& options, RunContext& context, const Target& target, const Wiring& wiring) { + if (options.video.path.empty()) { + return nullptr; + } + + VideoConfig config = options.video; + config.camera = config.camera.empty() ? target.video_camera() : config.camera; + config.body = target.ground_truth().body; + config.ticks_per_frame = ticks_per_frame(config.fps, context.clock); + + std::string error; + auto recorder = VideoRecorder::create(context.world, config, wiring.overlay, wiring.variables, error); + + if (recorder == nullptr) { + std::cerr << "video disabled: " << error << '\n'; + return nullptr; + } + + std::cout << "recording " << config.path << " at " << config.width << "x" << config.height << " " << config.fps + << " fps, camera '" << config.camera << "', one frame every " << config.ticks_per_frame << " ticks\n"; + + return recorder; +} + +/** + * @brief Build what the target adds to the run. + * + * @param target Robot to run. + * @param firmware Thread that will run the program. + * @param robot The robot's description. + * @param config The maze's surfaces. + * @return The target's wiring. + */ +static Wiring wire(Target& target, FirmwareThread& firmware, const RobotDescription& robot, const MazeConfig& config) { + RunContext& context = target.context(); + const WorldInfo world{ + .robot = &robot, + .reflectance = [&context, config](int geom) { + const char* name = mj_id2name(context.world.model(), mjOBJ_GEOM, geom); + return Maze::reflectance(name == nullptr ? "" : name, config); + }, + }; + + return target.wire(firmware, world); +} + +/** + * @brief Collect the geoms attached to the robot's body itself, whose contacts are collisions. + * + * @note The wheels hang on bodies of their own, so they are not among them. + * + * @param world The world. + * @param config The robot's ground truth, naming its body. + * @return Geom ids. + */ +static std::vector chassis_geoms(const MujocoWorld& world, const GroundTruthConfig& config) { + const mjModel* model = world.model(); + const int root = world.require_id(mjOBJ_BODY, config.body); + const std::span bodies(model->geom_bodyid, static_cast(model->ngeom)); + std::vector geoms; + + for (int geom = 0; geom < model->ngeom; geom++) { + if (at(bodies, static_cast(geom)) == root) { + geoms.push_back(geom); + } + } + + return geoms; +} + +Application::Application(const CliOptions& options, Target& target) : + options{options}, + target{target}, + scenario{read_scenario(options, target, this->scenario_path)}, + robot{RobotDescription::load(target.robot_file())}, + maze_path{resolve_maze(maze_name(options, this->scenario))}, + context{prepare(this->robot, this->maze_path, this->maze_config, options, this->scenario, target)}, + ticks{ticks_of(options, this->scenario, this->context)}, + firmware{target.program()}, + wiring{wire(target, this->firmware, this->robot, this->maze_config)}, + player{this->scenario, this->wiring.hooks, this->context.serial, this->wiring.variables}, + events{ + chassis_geoms(this->context.world, target.ground_truth()), + {this->context.world.require_id(mjOBJ_GEOM, std::string{Maze::body_name} + "_floor")}, + this->wiring.hooks.state_names, + this->wiring.variables + }, + device_columns{this->context.devices}, + recorder{this->context.world, target.ground_truth(), options.out / "data.csv", options.record_every}, + video{make_video_recorder(options, this->context, target, this->wiring)}, + viewer{make_viewer(options, this->context, target, this->wiring, this->player)}, + monitor{make_monitor(options, this->context.serial)}, + simulation{this->context, this->firmware} { + for (ColumnSource* source : this->wiring.columns) { + this->recorder.add_source(*source); + } + + this->recorder.add_source(this->device_columns); + + this->simulation.add_listener(CrashReporter::instance()); + this->simulation.add_listener(this->player); + + if (this->monitor != nullptr) { + this->simulation.add_listener(*this->monitor); + } + + this->simulation.add_listener(this->events); + this->simulation.add_listener(this->recorder); + + if (this->video != nullptr) { + this->simulation.add_listener(*this->video); + } + + if (this->viewer != nullptr) { + this->simulation.add_listener(*this->viewer); + } + + std::ofstream(options.out / "model.xml") << this->context.world.composed_xml(); +} + +Application::~Application() { + this->target.unwire(); +} + +void Application::run(std::span arguments) { + const auto wall_start = std::chrono::steady_clock::now(); + + CrashReporter::install(); + this->simulation.run(this->ticks); + + const auto wall_seconds = std::chrono::duration(std::chrono::steady_clock::now() - wall_start).count(); + const int64_t warnings = GroundTruth::warnings_total(this->context.world); + const int body = this->context.world.require_id(mjOBJ_BODY, this->target.ground_truth().body); + const std::span positions( + this->context.world.data()->xpos, 3 * static_cast(this->context.world.model()->nbody) + ); + + const RunMetadata metadata{ + .target = this->target.name(), + .target_dir = this->target.directory().string(), + .firmware_sha = this->target.firmware_sha(), + .robot_path = this->target.robot_file().string(), + .robot_sha256 = RunMetadata::sha256_of(this->target.robot_file()), + .scenario_path = this->scenario_path.string(), + .maze_path = this->maze_path.string(), + .model_sha256 = RunMetadata::sha256_of_text(this->context.world.composed_xml()), + .mujoco_version = mj_versionString(), + .compiler = MICRAS_COMPILER_ID " " MICRAS_COMPILER_VERSION, + .build_type = MICRAS_BUILD_TYPE, + .args = RunMetadata::join_args(arguments), + .seed = this->context.noise.seed, + .ideal = this->context.noise.ideal, + .loop_time_us = this->target.loop_time_us(), + .timestep = this->context.world.timestep(), + .steps_per_tick = this->context.clock.steps_per_tick(), + .record_every = this->options.record_every, + .requested_ticks = this->ticks, + .ticks = this->simulation.completed_ticks(), + .sim_time = this->context.world.time(), + .stopped_at = this->player.stopped_at().value_or(-1.0), + .final_z = at(positions, (3 * static_cast(body)) + 2), + .target_fields = this->target.metadata(), + .warnings_total = warnings, + .serial_dropped_bytes = this->context.serial.dropped_bytes(), + .bridge_dropped_frames = this->monitor == nullptr ? 0 : this->monitor->dropped_frames(), + .interactive = (this->viewer != nullptr and this->viewer->was_interactive()) or + (this->monitor != nullptr and this->monitor->was_interactive()), + .events = this->events.entries(), + }; + metadata.write(this->options.out / "meta.json"); + + std::cout << "ticks=" << this->simulation.completed_ticks() << "/" << this->ticks + << " sim_time=" << this->context.world.time() << "s wall=" << wall_seconds << "s"; + + for (const MetadataField& field : metadata.target_fields) { + std::cout << " " << field.name << "=" << field.value; + } + + std::cout << " collisions=" << this->events.collisions() << " warnings=" << warnings; + + if (this->video != nullptr) { + std::cout << " video_frames=" << this->video->frame_count(); + } + + std::cout << '\n'; +} + +int run(std::span arguments, Target& target) { + const std::vector target_options = target.options(); + const std::string program = + arguments.empty() ? target.name() : std::filesystem::path(arguments.front()).filename().string(); + + try { + std::cout << "MuJoCo " << mj_versionString() << '\n'; + + if (mj_version() != mjVERSION_HEADER) { + std::cerr << "MuJoCo library/header version mismatch: " << mj_version() << " != " << mjVERSION_HEADER + << '\n'; + return 1; + } + + const CliOptions options = Cli::parse(arguments, target_options); + std::filesystem::create_directories(options.out); + + Application application(options, target); + application.run(arguments); + return 0; + } catch (const std::exception& error) { + std::cerr << "error: " << error.what() << '\n' << Cli::usage(program, target_options); + return 1; + } +} +} // namespace micras::sim diff --git a/app/src/cli.cpp b/app/src/cli.cpp new file mode 100644 index 0000000..6380b99 --- /dev/null +++ b/app/src/cli.cpp @@ -0,0 +1,277 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/app/cli.hpp" +#include "micras/sim/app/wiring.hpp" +#include "micras/sim/core/span_at.hpp" + +namespace micras::sim { +/** + * @brief Read the value that follows an option. + * + * @param arguments Whole command line. + * @param index Index of the option, advanced to the value. + * @return The value. + */ +static std::string take_value(std::span arguments, std::size_t& index) { + if (index + 1 >= arguments.size()) { + throw std::runtime_error("missing value for option " + std::string(at(arguments, index))); + } + + return at(arguments, ++index); +} + +/** + * @brief Parse a number, naming the option and rejecting trailing rubbish. + * + * @tparam T Type to parse. + * @param value Number as spelled on the command line. + * @param option Name of the option, for the error message. + * @return The number. + */ +template +static T require_number(const std::string& value, const std::string& option) { + T number{}; + const std::span digits(value); + const char* const end = std::to_address(digits.end()); + const auto result = std::from_chars(digits.data(), end, number); + + if (result.ec != std::errc{} or result.ptr != end) { + throw std::runtime_error(option + " expects a number, got '" + value + "'"); + } + + return number; +} + +/** + * @brief Parse a size spelled as x. + * + * @param value Size as spelled on the command line. + * @param option Name of the option, for the error message. + * @param width Width to fill. + * @param height Height to fill. + */ +static void parse_size(const std::string& value, const std::string& option, int& width, int& height) { + const std::size_t cross = value.find('x'); + + if (cross == std::string::npos) { + throw std::runtime_error(option + " must look like 1280x720, got " + value); + } + + width = require_number(value.substr(0, cross), option); + height = require_number(value.substr(cross + 1), option); +} + +namespace { +/** + * @brief Reads the value that follows the option being applied. + */ +using ValueReader = std::function; +} // namespace + +/** + * @brief Apply one of the window options. + * + * @param argument Name of the option. + * @param value Reader of the value that follows it. + * @param options Options to fill. + * @return False when the name is not a window option. + */ +static bool apply_viewer_option(const std::string& argument, const ValueReader& value, CliOptions& options) { + if (argument == "--viewer") { + options.viewer_enabled = true; + } else if (argument == "--viewer-camera") { + options.viewer.camera = value(); + } else if (argument == "--viewer-fps") { + options.viewer_fps = require_number(value(), argument); + } else if (argument == "--viewer-size") { + parse_size(value(), "--viewer-size", options.viewer.width, options.viewer.height); + } else { + return false; + } + + return true; +} + +/** + * @brief Apply one of the monitor bridge options. + * + * @param argument Name of the option. + * @param value Reader of the value that follows it. + * @param options Options to fill. + * @return False when the name is not a monitor option. + */ +static bool apply_monitor_option(const std::string& argument, const ValueReader& value, CliOptions& options) { + if (argument == "--monitor") { + options.monitor_enabled = true; + } else if (argument == "--monitor-port") { + options.monitor_port = require_number(value(), argument); + } else { + return false; + } + + return true; +} + +/** + * @brief Apply one of the recording options. + * + * @param argument Name of the option. + * @param value Reader of the value that follows it. + * @param options Options to fill. + * @return False when the name is not a recording option. + */ +static bool apply_video_option(const std::string& argument, const ValueReader& value, CliOptions& options) { + if (argument == "--video") { + options.video.path = value(); + } else if (argument == "--video-fps") { + options.video.fps = require_number(value(), argument); + } else if (argument == "--video-camera") { + options.video.camera = value(); + } else if (argument == "--video-size") { + parse_size(value(), "--video-size", options.video.width, options.video.height); + } else if (argument == "--video-trail") { + options.video.trail = true; + } else { + return false; + } + + return true; +} + +/** + * @brief Reject an option combination the run could not honor. + * + * @param options Options as parsed. + */ +static void validate(const CliOptions& options) { + if (options.out.empty()) { + throw std::runtime_error("--out is required"); + } + + if (options.ticks.has_value() and *options.ticks == 0) { + throw std::runtime_error("--ticks must be at least 1"); + } + + if (options.seconds.has_value() and not(*options.seconds > 0.0)) { + throw std::runtime_error("--seconds must be strictly positive, got " + std::to_string(*options.seconds)); + } + + if (options.record_every == 0) { + throw std::runtime_error("--record-every must be at least 1"); + } + + if (options.viewer_enabled) { + if (options.viewer.width < 1 or options.viewer.height < 1) { + throw std::runtime_error("--viewer-size must be strictly positive"); + } + + if (options.viewer_fps < 1) { + throw std::runtime_error("--viewer-fps must be at least 1"); + } + } + + if (options.monitor_enabled and (options.monitor_port < 1 or options.monitor_port > 65535)) { + throw std::runtime_error("--monitor-port must be a TCP port"); + } + + if (options.video.path.empty()) { + return; + } + + if (options.video.fps < 1) { + throw std::runtime_error("--video-fps must be at least 1"); + } + + if (options.video.width < 1 or options.video.height < 1) { + throw std::runtime_error("--video-size must be strictly positive"); + } +} + +/** + * @brief Apply one option to the options being built. + * + * @param argument Name of the option. + * @param value Reader of the value that follows it. + * @param options Options to fill. + * @return False when the name is not an option at all. + */ +static bool apply_option(const std::string& argument, const ValueReader& value, CliOptions& options) { + if (argument == "--out") { + options.out = value(); + } else if (argument == "--scenario") { + options.scenario = value(); + } else if (argument == "--maze") { + options.maze = value(); + } else if (argument == "--seconds") { + options.seconds = require_number(value(), argument); + } else if (argument == "--ticks") { + options.ticks = require_number(value(), argument); + } else if (argument == "--seed") { + options.seed = require_number(value(), argument); + } else if (argument == "--ideal") { + options.ideal = true; + } else if (argument == "--record-every") { + options.record_every = require_number(value(), argument); + } else { + return apply_video_option(argument, value, options) or apply_viewer_option(argument, value, options) or + apply_monitor_option(argument, value, options); + } + + return true; +} + +std::string Cli::usage(std::string_view program, std::span target_options) { + const std::string indent(7 + program.size(), ' '); + std::string text = "usage: " + std::string(program) + " --out [--scenario ] [--maze |]\n" + + indent + + "[--seconds | --ticks ] [--seed ] [--ideal] [--record-every ]\n" + + indent + "[--video ] [--video-fps ] [--video-camera |free]\n" + indent + + "[--video-size x] [--video-trail]\n" + indent + + "[--viewer] [--viewer-camera |free] [--viewer-fps ]\n" + indent + + "[--viewer-size x]\n" + indent + "[--monitor] [--monitor-port ]\n"; + + for (const CliOption& option : target_options) { + text += indent + "[" + option.name + (option.argument.empty() ? "" : " " + option.argument) + "]\n"; + } + + return text; +} + +CliOptions Cli::parse(std::span arguments, std::span target_options) { + CliOptions options; + + for (std::size_t i = 1; i < arguments.size(); i++) { + const std::string argument = at(arguments, i); + const ValueReader value = [&arguments, &i] { return take_value(arguments, i); }; + + if (apply_option(argument, value, options)) { + continue; + } + + const auto option = std::ranges::find(target_options, argument, &CliOption::name); + + if (option == target_options.end()) { + throw std::runtime_error("unknown option " + argument); + } + + option->apply(option->argument.empty() ? std::string{} : value()); + } + + validate(options); + return options; +} +} // namespace micras::sim diff --git a/src/sim/app/crash_reporter.cpp b/app/src/crash_reporter.cpp similarity index 94% rename from src/sim/app/crash_reporter.cpp rename to app/src/crash_reporter.cpp index 71705ce..f423144 100644 --- a/src/sim/app/crash_reporter.cpp +++ b/app/src/crash_reporter.cpp @@ -4,11 +4,13 @@ #include #include +#include #include #include #include "micras/sim/app/crash_reporter.hpp" +#include "micras/sim/core/simulation.hpp" namespace micras::sim { namespace { @@ -17,7 +19,9 @@ namespace { */ // NOLINTNEXTLINE(*-avoid-non-const-global-variables): the only state a signal handler may read. volatile std::sig_atomic_t running_tick = -1; +} // namespace +extern "C" { /** * @brief Name the tick a fatal signal interrupted and re-raise it. * @@ -27,7 +31,7 @@ volatile std::sig_atomic_t running_tick = -1; * * @param signal_number Signal being handled. */ -extern "C" void report_and_reraise(int signal_number) { +static void report_and_reraise(int signal_number) { constexpr std::array prefix{"firmware crashed at tick "}; std::array digits{}; int64_t tick = running_tick; @@ -59,7 +63,7 @@ extern "C" void report_and_reraise(int signal_number) { std::signal(signal_number, SIG_DFL); std::raise(signal_number); } -} // namespace +} CrashReporter& CrashReporter::instance() { static CrashReporter reporter; diff --git a/baseline/v0/explore_legacy/data.csv.gz b/baseline/v0/explore_legacy/data.csv.gz deleted file mode 100644 index 10e65fb..0000000 Binary files a/baseline/v0/explore_legacy/data.csv.gz and /dev/null differ diff --git a/baseline/v0/explore_legacy/meta.json b/baseline/v0/explore_legacy/meta.json deleted file mode 100644 index 4dbd490..0000000 --- a/baseline/v0/explore_legacy/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_legacy.xml", - "model_sha256": "c1e24df2fce989971ae83b307cf96e3070506d1c43830f5442482d6e4f95523c", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_legacy.xml --command explore --seconds 8 --out runs/explore_legacy", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.00114835, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v0/explore_v2/data.csv.gz b/baseline/v0/explore_v2/data.csv.gz deleted file mode 100644 index ac7d637..0000000 Binary files a/baseline/v0/explore_v2/data.csv.gz and /dev/null differ diff --git a/baseline/v0/explore_v2/meta.json b/baseline/v0/explore_v2/meta.json deleted file mode 100644 index 801301a..0000000 --- a/baseline/v0/explore_v2/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --command explore --seconds 8 --out runs/explore_v2", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.000974307, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v0/idle/data.csv.gz b/baseline/v0/idle/data.csv.gz deleted file mode 100644 index 28d97d8..0000000 Binary files a/baseline/v0/idle/data.csv.gz and /dev/null differ diff --git a/baseline/v0/idle/meta.json b/baseline/v0/idle/meta.json deleted file mode 100644 index 558c907..0000000 --- a/baseline/v0/idle/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --seconds 4 --out runs/idle", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 3838, - "sim_time": 3.9992, - "final_z": 0.000806901, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v1/explore_legacy/data.csv.gz b/baseline/v1/explore_legacy/data.csv.gz deleted file mode 100644 index 3818840..0000000 Binary files a/baseline/v1/explore_legacy/data.csv.gz and /dev/null differ diff --git a/baseline/v1/explore_legacy/meta.json b/baseline/v1/explore_legacy/meta.json deleted file mode 100644 index 4dbd490..0000000 --- a/baseline/v1/explore_legacy/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_legacy.xml", - "model_sha256": "c1e24df2fce989971ae83b307cf96e3070506d1c43830f5442482d6e4f95523c", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_legacy.xml --command explore --seconds 8 --out runs/explore_legacy", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.00114835, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v1/explore_v2/data.csv.gz b/baseline/v1/explore_v2/data.csv.gz deleted file mode 100644 index a9c9aa4..0000000 Binary files a/baseline/v1/explore_v2/data.csv.gz and /dev/null differ diff --git a/baseline/v1/explore_v2/meta.json b/baseline/v1/explore_v2/meta.json deleted file mode 100644 index 801301a..0000000 --- a/baseline/v1/explore_v2/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --command explore --seconds 8 --out runs/explore_v2", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.000974307, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v1/idle/data.csv.gz b/baseline/v1/idle/data.csv.gz deleted file mode 100644 index 79c048e..0000000 Binary files a/baseline/v1/idle/data.csv.gz and /dev/null differ diff --git a/baseline/v1/idle/meta.json b/baseline/v1/idle/meta.json deleted file mode 100644 index 558c907..0000000 --- a/baseline/v1/idle/meta.json +++ /dev/null @@ -1,19 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --seconds 4 --out runs/idle", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "ticks": 3838, - "sim_time": 3.9992, - "final_z": 0.000806901, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0 -} diff --git a/baseline/v2/explore_legacy/data.csv.gz b/baseline/v2/explore_legacy/data.csv.gz deleted file mode 100644 index 073ac4b..0000000 Binary files a/baseline/v2/explore_legacy/data.csv.gz and /dev/null differ diff --git a/baseline/v2/explore_legacy/meta.json b/baseline/v2/explore_legacy/meta.json deleted file mode 100644 index c890c04..0000000 --- a/baseline/v2/explore_legacy/meta.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_legacy.xml", - "model_sha256": "c1e24df2fce989971ae83b307cf96e3070506d1c43830f5442482d6e4f95523c", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_legacy.xml --command explore --seconds 8 --out runs/explore_legacy", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "requested_ticks": 7677, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.00114835, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0, - "interactive": false -} diff --git a/baseline/v2/explore_v2/data.csv.gz b/baseline/v2/explore_v2/data.csv.gz deleted file mode 100644 index 3c93683..0000000 Binary files a/baseline/v2/explore_v2/data.csv.gz and /dev/null differ diff --git a/baseline/v2/explore_v2/meta.json b/baseline/v2/explore_v2/meta.json deleted file mode 100644 index 4dd7959..0000000 --- a/baseline/v2/explore_v2/meta.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --command explore --seconds 8 --out runs/explore_v2", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "requested_ticks": 7677, - "ticks": 7677, - "sim_time": 7.99943, - "final_z": 0.000974307, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0, - "interactive": false -} diff --git a/baseline/v2/idle/data.csv.gz b/baseline/v2/idle/data.csv.gz deleted file mode 100644 index 453308e..0000000 Binary files a/baseline/v2/idle/data.csv.gz and /dev/null differ diff --git a/baseline/v2/idle/meta.json b/baseline/v2/idle/meta.json deleted file mode 100644 index ad91965..0000000 --- a/baseline/v2/idle/meta.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "firmware_sha": "ad342541267bb3c8db263630f27fc7edafba5b09", - "model_path": "models/robot_v2.xml", - "model_sha256": "3ef384dfab2130e13e686db3a8030e250a780b8dca03ac6dc451a4836baa4786", - "maze_path": "", - "mujoco_version": "3.3.6", - "compiler": "GNU 13.3.0", - "build_type": "RelWithDebInfo", - "args": "--model models/robot_v2.xml --seconds 4 --out runs/idle", - "loop_time_us": 1042, - "timestep": 0.000521, - "steps_per_tick": 2, - "requested_ticks": 3838, - "ticks": 3838, - "sim_time": 3.9992, - "final_z": 0.000806901, - "pool_columns": 29, - "telemetry_resyncs": 0, - "warnings_total": 0, - "interactive": false -} diff --git a/include/micras/sim/bridge/monitor_bridge.hpp b/bridge/include/micras/sim/bridge/monitor_bridge.hpp similarity index 63% rename from include/micras/sim/bridge/monitor_bridge.hpp rename to bridge/include/micras/sim/bridge/monitor_bridge.hpp index ca9929a..a9eaa67 100644 --- a/include/micras/sim/bridge/monitor_bridge.hpp +++ b/bridge/include/micras/sim/bridge/monitor_bridge.hpp @@ -8,26 +8,22 @@ #define MICRAS_SIM_BRIDGE_MONITOR_BRIDGE_HPP #include -#include #include +#include #include #include #include "micras/sim/bridge/web_socket_server.hpp" #include "micras/sim/core/serial_bus.hpp" #include "micras/sim/core/simulation.hpp" -#include "micras/sim/telemetry/packet_framer.hpp" namespace micras::sim { /** - * @brief Carries the firmware's bytes to a monitor and its packets back. + * @brief Carries the firmware's bytes to a monitor and the monitor's bytes back. * - * @note The monitor talks the same packet protocol the real radio does, so it - * connects to the simulation without being told it is one. Everything it - * sends is framed and queued on the bus, which hands the firmware one - * packet per tick, so a burst from the monitor cannot change how many - * packets a tick delivers and the run stays as reproducible as the - * monitor's own input allows. + * @note Bytes only: the bridge frames nothing. What a monitor sends is collected + * on a server thread and queued for the firmware on the simulation thread, + * before the tick. The firmware's output leaves as one frame per tick. */ class MonitorBridge : public ISerialListener, public IRunListener { public: @@ -46,7 +42,7 @@ class MonitorBridge : public ISerialListener, public IRunListener { MonitorBridge& operator=(MonitorBridge&&) = delete; /** - * @brief Stop the server and leave the bus. + * @brief Leave the bus; the server stops as the last member goes. */ ~MonitorBridge() override; @@ -58,11 +54,21 @@ class MonitorBridge : public ISerialListener, public IRunListener { bool is_open() const { return this->open; } /** - * @brief Get how many monitors are connected. + * @brief Check whether a monitor sent anything the firmware was handed. * - * @return Number of clients. + * @note A run that received bytes from outside is no longer reproducible from + * its arguments alone, so the application marks it interactive. + * + * @return True once the first bytes from a monitor were handed over. + */ + bool was_interactive() const { return this->received; } + + /** + * @brief Get how many frames never reached the monitors. + * + * @return Frames dropped because the monitors did not keep up. */ - std::size_t client_count() const { return this->server.client_count(); } + uint64_t dropped_frames() const { return this->server.dropped_frames(); } /** * @brief Collect the bytes the firmware wrote during the tick. @@ -72,7 +78,7 @@ class MonitorBridge : public ISerialListener, public IRunListener { void on_firmware_bytes(std::span bytes) override; /** - * @brief Hand the monitor's packets to the firmware, one tick's worth. + * @brief Queue what the monitors sent for the firmware. * * @param simulation Run about to advance. * @return Always RunControl::RUN; a monitor never stops a run. @@ -95,14 +101,14 @@ class MonitorBridge : public ISerialListener, public IRunListener { SerialBus& serial; // NOLINT(*-avoid-const-or-ref-data-members) /** - * @brief The server itself. + * @brief Whether the port was taken. */ - WebSocketServer server; + bool open{false}; /** - * @brief Whether the port was taken. + * @brief Whether bytes from a monitor were handed over. */ - bool open{false}; + bool received{false}; /** * @brief Bytes the firmware wrote during the current tick. @@ -110,17 +116,22 @@ class MonitorBridge : public ISerialListener, public IRunListener { std::vector outgoing; /** - * @brief Guards everything the server threads touch. + * @brief Guards the incoming bytes, which a server thread appends to. */ mutable std::mutex mutex; /** - * @brief Frames what the monitors send, on the simulation thread. + * @brief Bytes the monitors sent since the last tick. + */ + std::vector incoming; + + /** + * @brief The server itself. * - * @note Guarded, because bytes arrive on a server thread and are framed - * when the simulation drains them. + * @note Declared last so it is destroyed first: its threads call back into + * the mutex and the buffer above, which must outlive them. */ - PacketFramer incoming; + WebSocketServer server; }; } // namespace micras::sim diff --git a/include/micras/sim/bridge/web_socket_server.hpp b/bridge/include/micras/sim/bridge/web_socket_server.hpp similarity index 66% rename from include/micras/sim/bridge/web_socket_server.hpp rename to bridge/include/micras/sim/bridge/web_socket_server.hpp index 1af033d..49ae532 100644 --- a/include/micras/sim/bridge/web_socket_server.hpp +++ b/bridge/include/micras/sim/bridge/web_socket_server.hpp @@ -12,15 +12,14 @@ #include #include #include -#include namespace micras::sim { /** * @brief Serves binary frames to whoever connects, and hands back what they send. * - * @note A facade on purpose: the harness only needs start, stop, broadcast and - * a callback, so the library behind it can be replaced without touching - * anything that uses it. Callbacks arrive on the server's own threads. + * @note A facade on purpose: it keeps IXWebSocket out of every header, and lets + * -DMICRAS_SIM_BRIDGE=OFF build a stub in its place. Callbacks arrive on the + * server's own threads. */ class WebSocketServer { public: @@ -63,18 +62,28 @@ class WebSocketServer { void stop(); /** - * @brief Send one binary frame to every connected client. + * @brief Queue one binary frame for every connected client. + * + * @note Never blocks on a client. The frame goes into a bounded queue that a + * sender thread drains, because the library writes synchronously and + * waits without a deadline for a client that stopped reading. When the + * queue is full the oldest frame is dropped and counted. * * @param bytes Frame payload. */ void broadcast(std::span bytes); /** - * @brief Get how many clients are connected. + * @brief Get how many frames were dropped because the send queue was full. * - * @return Number of clients. + * @return Number of dropped frames, always 0 when every client keeps up. + */ + uint64_t dropped_frames() const; + + /** + * @brief Largest number of frames allowed to wait for the sender thread. */ - std::size_t client_count() const; + static constexpr std::size_t max_queued_frames{256}; private: struct Impl; diff --git a/src/sim/bridge/monitor_bridge.cpp b/bridge/src/monitor_bridge.cpp similarity index 65% rename from src/sim/bridge/monitor_bridge.cpp rename to bridge/src/monitor_bridge.cpp index 5345d79..080cc75 100644 --- a/src/sim/bridge/monitor_bridge.cpp +++ b/bridge/src/monitor_bridge.cpp @@ -2,13 +2,21 @@ * @file */ +#include +#include +#include +#include +#include + #include "micras/sim/bridge/monitor_bridge.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/simulation.hpp" namespace micras::sim { MonitorBridge::MonitorBridge(SerialBus& serial, int port, std::string& error) : serial{serial} { this->server.set_on_binary([this](std::span bytes) { - const std::lock_guard lock(this->mutex); - this->incoming.push(bytes); + const std::scoped_lock lock(this->mutex); + this->incoming.insert(this->incoming.end(), bytes.begin(), bytes.end()); }); if (this->server.start(port, error)) { @@ -18,8 +26,6 @@ MonitorBridge::MonitorBridge(SerialBus& serial, int port, std::string& error) : } MonitorBridge::~MonitorBridge() { - this->server.stop(); - if (this->open) { this->serial.remove_listener(*this); } @@ -30,15 +36,16 @@ void MonitorBridge::on_firmware_bytes(std::span bytes) { } RunControl MonitorBridge::on_before_tick(const Simulation& /*simulation*/) { - std::vector> packets; + std::vector bytes; { - const std::lock_guard lock(this->mutex); - packets = this->incoming.drain(); + const std::scoped_lock lock(this->mutex); + bytes.swap(this->incoming); } - for (const auto& packet : packets) { - this->serial.queue_for_firmware(packet); + if (not bytes.empty()) { + this->received = true; + this->serial.queue_for_firmware(bytes); } return RunControl::RUN; diff --git a/bridge/src/web_socket_server.cpp b/bridge/src/web_socket_server.cpp new file mode 100644 index 0000000..ea1889a --- /dev/null +++ b/bridge/src/web_socket_server.cpp @@ -0,0 +1,210 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/sim/bridge/web_socket_server.hpp" + +#ifdef MICRAS_SIM_BRIDGE + + #include + #include + #include + #include + + #include + #include + #include + #include + #include + +namespace micras::sim { +/** + * @brief Everything IXWebSocket needs, kept out of the header. + */ +struct WebSocketServer::Impl { + /** + * @brief Send every queued frame until asked to stop. + * + * @note Runs on its own thread, so a client that stops reading blocks this + * thread and never the one that calls broadcast(). + */ + void send_loop(); + + std::unique_ptr server; + BinaryHandler on_binary; + + /** + * @brief Guards the queue, the drop counter and the stop flag. + */ + std::mutex mutex; + + /** + * @brief Signals a queued frame or a request to stop. + */ + std::condition_variable queued; + + /** + * @brief Frames waiting for the sender thread, oldest first. + */ + std::deque frames; + + /** + * @brief Frames dropped because the queue was full. + */ + uint64_t dropped{0}; + + /** + * @brief Set when the sender thread must leave. + */ + bool stopping{false}; + + /** + * @brief Drains the queue into the clients. + */ + std::thread sender; +}; + +void WebSocketServer::Impl::send_loop() { + while (true) { + std::string frame; + + { + std::unique_lock lock(this->mutex); + this->queued.wait(lock, [this] { return this->stopping or not this->frames.empty(); }); + + if (this->stopping) { + return; + } + + frame = std::move(this->frames.front()); + this->frames.pop_front(); + } + + for (const auto& client : this->server->getClients()) { + client->sendBinary(frame); + } + } +} + +WebSocketServer::WebSocketServer() : impl{std::make_unique()} { } + +WebSocketServer::~WebSocketServer() { + this->stop(); +} + +void WebSocketServer::set_on_binary(BinaryHandler handler) { + this->impl->on_binary = std::move(handler); +} + +bool WebSocketServer::start(int port, std::string& error) { + this->impl->server = std::make_unique(port); + this->impl->server->disablePerMessageDeflate(); + + this->impl->server->setOnClientMessageCallback( + [this](const std::shared_ptr&, ix::WebSocket&, const ix::WebSocketMessagePtr& message) { + if (message->type != ix::WebSocketMessageType::Message or not message->binary) { + return; + } + + if (this->impl->on_binary) { + const std::span bytes( + reinterpret_cast(message->str.data()), // NOLINT(*-reinterpret-cast): byte view. + message->str.size() + ); + this->impl->on_binary(bytes); + } + } + ); + + const auto result = this->impl->server->listen(); + + if (not result.first) { + error = result.second; + this->impl->server.reset(); + return false; + } + + this->impl->server->start(); + this->impl->stopping = false; + this->impl->sender = std::thread(&Impl::send_loop, this->impl.get()); + return true; +} + +void WebSocketServer::stop() { + if (this->impl->server == nullptr) { + return; + } + + { + const std::scoped_lock lock(this->impl->mutex); + this->impl->stopping = true; + this->impl->frames.clear(); + } + + this->impl->queued.notify_all(); + this->impl->server->stop(); + this->impl->sender.join(); + this->impl->server.reset(); +} + +void WebSocketServer::broadcast(std::span bytes) { + if (this->impl->server == nullptr or bytes.empty()) { + return; + } + + { + const std::scoped_lock lock(this->impl->mutex); + + if (this->impl->frames.size() >= max_queued_frames) { + this->impl->frames.pop_front(); + this->impl->dropped++; + } + + // NOLINTNEXTLINE(*-reinterpret-cast): IXWebSocket takes a std::string as its byte buffer. + this->impl->frames.emplace_back(reinterpret_cast(bytes.data()), bytes.size()); + } + + this->impl->queued.notify_one(); +} + +uint64_t WebSocketServer::dropped_frames() const { + const std::scoped_lock lock(this->impl->mutex); + return this->impl->dropped; +} +} // namespace micras::sim + +#else // MICRAS_SIM_BRIDGE + +namespace micras::sim { +/** + * @brief Nothing to hide when the bridge is compiled out. + */ +struct WebSocketServer::Impl { }; + +WebSocketServer::WebSocketServer() : impl{std::make_unique()} { } + +WebSocketServer::~WebSocketServer() = default; + +void WebSocketServer::set_on_binary(BinaryHandler /*handler*/) { } + +bool WebSocketServer::start(int /*port*/, std::string& error) { + error = "this binary was built with -DMICRAS_SIM_BRIDGE=OFF"; + return false; +} + +void WebSocketServer::stop() { } + +void WebSocketServer::broadcast(std::span /*bytes*/) { } + +uint64_t WebSocketServer::dropped_frames() const { + return 0; +} +} // namespace micras::sim + +#endif // MICRAS_SIM_BRIDGE diff --git a/cmake/checks.cmake b/cmake/checks.cmake new file mode 100644 index 0000000..7de28dd --- /dev/null +++ b/cmake/checks.cmake @@ -0,0 +1,65 @@ +############################################################################### +## The simulator's checks, as targets over the scripts of scripts/ (top level only) +############################################################################### + +# micras_sim_collect_targets( ) +# +# Every executable and library the simulator itself defines in and below, not the fetched +# dependencies', so that a check can depend on the whole build. +function(micras_sim_collect_targets VARIABLE DIRECTORY) + get_property(TARGETS DIRECTORY "${DIRECTORY}" PROPERTY BUILDSYSTEM_TARGETS) + get_property(SUBDIRECTORIES DIRECTORY "${DIRECTORY}" PROPERTY SUBDIRECTORIES) + set(COLLECTED ${${VARIABLE}}) + + foreach(TARGET_NAME IN LISTS TARGETS) + get_target_property(TYPE ${TARGET_NAME} TYPE) + + if(TYPE MATCHES "^(EXECUTABLE|STATIC_LIBRARY|SHARED_LIBRARY)$") + list(APPEND COLLECTED ${TARGET_NAME}) + endif() + endforeach() + + foreach(SUBDIRECTORY IN LISTS SUBDIRECTORIES) + if(SUBDIRECTORY MATCHES "^${PROJECT_BINARY_DIR}/_deps/" OR NOT SUBDIRECTORY MATCHES "^${PROJECT_SOURCE_DIR}/") + continue() + endif() + + micras_sim_collect_targets(COLLECTED "${SUBDIRECTORY}") + endforeach() + + set(${VARIABLE} ${COLLECTED} PARENT_SCOPE) +endfunction() + +set(MICRAS_SIM_SCRIPTS_DIR "${PROJECT_SOURCE_DIR}/scripts") + +# The options check reuses what this build fetched, so it downloads nothing. +add_custom_target(micras_sim_check_options + COMMAND "${MICRAS_SIM_SCRIPTS_DIR}/check_options.sh" + "${PROJECT_SOURCE_DIR}" "${PROJECT_BINARY_DIR}-minimal" "${CMAKE_GENERATOR}" + "${CMAKE_C_COMPILER}" "${CMAKE_CXX_COMPILER}" "${CMAKE_BUILD_TYPE}" "${MICRAS_SIM_WERROR}" + "-DMUJOCO_DIR=${MUJOCO_ROOT}" + "-DFETCHCONTENT_SOURCE_DIR_PICOSHA2=${picosha2_SOURCE_DIR}" + "-DFETCHCONTENT_SOURCE_DIR_TOMLPLUSPLUS=${tomlplusplus_SOURCE_DIR}" + COMMENT "Building with every option off" + VERBATIM USES_TERMINAL +) + +add_custom_target(micras_sim_check_generic + COMMAND "${MICRAS_SIM_SCRIPTS_DIR}/check_generic.sh" "${PROJECT_SOURCE_DIR}" + COMMENT "Checking that nothing outside targets/ names a robot" + VERBATIM USES_TERMINAL +) + +# The build, every unit test, the options, the generic check and the toy target's checks. +add_custom_target(micras_sim_check + COMMAND "${CMAKE_CTEST_COMMAND}" --test-dir "${PROJECT_BINARY_DIR}" --output-on-failure --parallel + COMMENT "Running the unit tests" + VERBATIM USES_TERMINAL +) + +micras_sim_collect_targets(MICRAS_SIM_BUILD_TARGETS "${PROJECT_SOURCE_DIR}") +add_dependencies(micras_sim_check micras_sim_check_options micras_sim_check_generic ${MICRAS_SIM_BUILD_TARGETS}) + +if(TARGET micras_sim_toy_check) + add_dependencies(micras_sim_check micras_sim_toy_check) +endif() diff --git a/cmake/firmware.cmake b/cmake/firmware.cmake deleted file mode 100644 index c31f668..0000000 --- a/cmake/firmware.cmake +++ /dev/null @@ -1,47 +0,0 @@ -############################################################################### -## Firmware shadow: the firmware compiled against the simulation proxies -############################################################################### - -set(MICRAS_FIRMWARE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/MicrasFirmware") - -execute_process( - COMMAND git -C "${MICRAS_FIRMWARE_DIR}" rev-parse HEAD - OUTPUT_VARIABLE MICRAS_FIRMWARE_SHA - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET -) - -if(NOT MICRAS_FIRMWARE_SHA) - set(MICRAS_FIRMWARE_SHA "unknown") -endif() - -# main.cpp is excluded because it pulls in the STM32 HAL; micras_hal and -# micras_proxy are replaced by include/micras/proxy + src/proxy. -file(GLOB FIRMWARE_SOURCES CONFIGURE_DEPENDS - "${MICRAS_FIRMWARE_DIR}/micras_core/src/*.cpp" - "${MICRAS_FIRMWARE_DIR}/micras_nav/src/*.cpp" - "${MICRAS_FIRMWARE_DIR}/micras_comm/src/*.cpp" -) -list(APPEND FIRMWARE_SOURCES - "${MICRAS_FIRMWARE_DIR}/src/micras.cpp" - "${MICRAS_FIRMWARE_DIR}/src/interface.cpp" -) - -file(GLOB PROXY_SOURCES CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/src/proxy/*.cpp") - -add_library(micras_firmware_shadow STATIC ${FIRMWARE_SOURCES} ${PROXY_SOURCES}) - -# Order matters: config/ and include/ must shadow the firmware headers. -target_include_directories(micras_firmware_shadow PUBLIC - "${CMAKE_CURRENT_SOURCE_DIR}/config" - "${CMAKE_CURRENT_SOURCE_DIR}/include" - "${MICRAS_FIRMWARE_DIR}/include" - "${MICRAS_FIRMWARE_DIR}/micras_nav/include" - "${MICRAS_FIRMWARE_DIR}/micras_comm/include" - "${MICRAS_FIRMWARE_DIR}/micras_core/include" -) - -target_link_libraries(micras_firmware_shadow PUBLIC micras_sim_core) - -# -Wall/-Wextra only on our own code; the firmware is pinned and out of scope. -set_source_files_properties(${PROXY_SOURCES} PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra") diff --git a/cmake/hardware_tests.cmake b/cmake/hardware_tests.cmake deleted file mode 100644 index 1149d8d..0000000 --- a/cmake/hardware_tests.cmake +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################### -## One executable per firmware hardware test -############################################################################### - -# The tests are compiled unchanged: their main is renamed so the harness can -# supply its own and run theirs inside the firmware thread. -function(micras_add_hardware_test NAME SOURCE) - set(TARGET "micras_${NAME}") - - add_executable(${TARGET} - "${MICRAS_FIRMWARE_DIR}/${SOURCE}" - "${CMAKE_SOURCE_DIR}/src/sim/app/hardware_test_main.cpp" - ) - - set_source_files_properties("${MICRAS_FIRMWARE_DIR}/${SOURCE}" - PROPERTIES COMPILE_DEFINITIONS "main=micras_test_main" - ) - - target_include_directories(${TARGET} PRIVATE "${CMAKE_SOURCE_DIR}/tests/include") - target_link_libraries(${TARGET} PRIVATE micras_sim_app) - - set_target_properties(${TARGET} PROPERTIES - OUTPUT_NAME ${NAME} - RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/hardware_tests" - BUILD_RPATH "${MUJOCO_DIR}/lib" - INSTALL_RPATH "${MUJOCO_DIR}/lib" - ) -endfunction() - -function(micras_add_hardware_tests) - foreach(SOURCE ${ARGN}) - get_filename_component(NAME "${SOURCE}" NAME_WLE) - micras_add_hardware_test("${NAME}" "${SOURCE}") - list(APPEND TARGETS "micras_${NAME}") - endforeach() - - add_custom_target(hardware_tests DEPENDS ${TARGETS}) -endfunction() diff --git a/cmake/lint.cmake b/cmake/lint.cmake index 020eabd..5f332b8 100644 --- a/cmake/lint.cmake +++ b/cmake/lint.cmake @@ -1,41 +1,60 @@ ############################################################################### -## Format and lint targets (clang-format / clang-tidy from the firmware configs) +## Format and lint targets, with the configuration shared by the three repositories ############################################################################### -find_program(CLANG_FORMAT clang-format) -find_program(CLANG_TIDY clang-tidy) +# The clang tools are found by their versioned names: another major version formats and lints +# differently, so a missing one is an error instead of a silent fallback to whatever is installed. +set(MICRAS_SIM_CLANG_VERSION 22) -function(micras_add_lint_targets) - set(SOURCES ${ARGN}) +foreach(TOOL clang-format clang-tidy run-clang-tidy clang-apply-replacements) + string(TOUPPER "MICRAS_${TOOL}" VARIABLE) + string(REPLACE "-" "_" VARIABLE "${VARIABLE}") + find_program(${VARIABLE} NAMES "${TOOL}-${MICRAS_SIM_CLANG_VERSION}") - if(NOT CLANG_FORMAT OR NOT CLANG_TIDY) - message(WARNING "clang-format/clang-tidy not found; the format and lint targets are unavailable") - return() + if(NOT ${VARIABLE}) + message(FATAL_ERROR + "${TOOL}-${MICRAS_SIM_CLANG_VERSION} was not found. The style targets need clang ${MICRAS_SIM_CLANG_VERSION}'s " + "clang-format, clang-tidy, run-clang-tidy and clang-apply-replacements by their versioned names.") endif() +endforeach() - add_custom_target(format - COMMAND "${CLANG_FORMAT}" -style=file -i ${SOURCES} - WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" - COMMENT "Formatting harness sources" +# micras_sim_add_lint_targets(FORMAT ... LINT ...) +# +# micras_sim_format and micras_sim_format_check cover FORMAT, micras_sim_lint and micras_sim_lint_fix run +# clang-tidy over the LINT sources, which must be in the compilation database. +function(micras_sim_add_lint_targets) + cmake_parse_arguments(PARSE_ARGV 0 MICRAS_SIM_STYLE "" "" "FORMAT;LINT") + + add_custom_target(micras_sim_format + COMMAND "${MICRAS_CLANG_FORMAT}" -style=file -i ${MICRAS_SIM_STYLE_FORMAT} + WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" + COMMENT "Formatting the simulator's sources" VERBATIM USES_TERMINAL ) - add_custom_target(format-check - COMMAND "${CLANG_FORMAT}" -style=file --dry-run --Werror ${SOURCES} - WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" - COMMENT "Checking harness formatting" + add_custom_target(micras_sim_format_check + COMMAND "${MICRAS_CLANG_FORMAT}" -style=file --dry-run --Werror ${MICRAS_SIM_STYLE_FORMAT} + WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" + COMMENT "Checking the simulator's formatting" VERBATIM USES_TERMINAL ) - list(FILTER SOURCES INCLUDE REGEX "\\.cpp$") + # The host compiler needs no extra argument, unlike the robot's cross compiler. + set(MICRAS_TIDY_EXTRA_ARGS "") + set(LINT_SCRIPT "${PROJECT_BINARY_DIR}/run_clang_tidy.sh") + configure_file("${PROJECT_SOURCE_DIR}/cmake/templates/run_clang_tidy.sh.in" "${LINT_SCRIPT}" @ONLY) + + add_custom_target(micras_sim_lint + COMMAND "${LINT_SCRIPT}" ${MICRAS_SIM_STYLE_LINT} + WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" + COMMENT "Linting the simulator's sources" + VERBATIM USES_TERMINAL + ) - # Anchored so the pinned firmware submodule headers are never linted. - add_custom_target(lint - COMMAND "${CLANG_TIDY}" --quiet --warnings-as-errors=* - "--header-filter=^${CMAKE_SOURCE_DIR}/(include|config|tests)/" - -p "${CMAKE_BINARY_DIR}" ${SOURCES} - WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" - COMMENT "Linting harness sources" + add_custom_target(micras_sim_lint_fix + COMMAND "${LINT_SCRIPT}" --fix ${MICRAS_SIM_STYLE_LINT} + WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}" + COMMENT "Linting the simulator's sources and applying the fixes" VERBATIM USES_TERMINAL ) endfunction() diff --git a/cmake/micras_warnings.cmake b/cmake/micras_warnings.cmake new file mode 100644 index 0000000..eb201b2 --- /dev/null +++ b/cmake/micras_warnings.cmake @@ -0,0 +1,57 @@ +############################################################################### +## Project Warning Flags +############################################################################### + +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte, and the firmware's CI compares them. The -Werror switch belongs to each repository +# (MICRAS_LIB_WERROR, MICRAS_SIM_WERROR, MICRAS_WERROR) and is passed to micras_apply_warnings. + +# The -Wduplicated-* and -Wuseless-cast flags are GCC only. -fno-common is a code generation flag: +# GCC's default for C and a no-op for C++, kept so that the robot's image stays the same. +set(MICRAS_WARNING_OPTIONS + -Wall + -Wextra + -Wpedantic + -Wcast-align + -Wcast-qual + -Wdouble-promotion + $<$:-Wduplicated-branches> + $<$:-Wduplicated-cond> + -Wfloat-conversion + -Wformat=2 + -Wimplicit-fallthrough + -Wmisleading-indentation + -Wnull-dereference + -Wsign-conversion + -Wundef + -Wunused-parameter + -fno-common + + # C++ only, they make the C compiler warn about the command line itself + $<$:-Wnon-virtual-dtor> + $<$:-Wold-style-cast> + $<$:-Woverloaded-virtual> + $<$,$>:-Wuseless-cast> +) + +# Kept globally as well, so that the function works in any directory, whichever of the three +# repositories included this file first +set_property(GLOBAL PROPERTY MICRAS_WARNING_OPTIONS ${MICRAS_WARNING_OPTIONS}) + +# micras_apply_warnings(... [WERROR ]) +# +# Applied per target, so that the generated sources are never compiled with them, and as options +# rather than through a target, which a static library would carry in its interface. The targets +# are real names, not aliases. +function(micras_apply_warnings) + cmake_parse_arguments(PARSE_ARGV 0 MICRAS_WARNINGS "" "WERROR" "") + get_property(WARNING_OPTIONS GLOBAL PROPERTY MICRAS_WARNING_OPTIONS) + + if(MICRAS_WARNINGS_WERROR) + list(APPEND WARNING_OPTIONS -Werror) + endif() + + foreach(TARGET_NAME ${MICRAS_WARNINGS_UNPARSED_ARGUMENTS}) + target_compile_options(${TARGET_NAME} PRIVATE ${WARNING_OPTIONS}) + endforeach() +endfunction() diff --git a/cmake/mujoco.cmake b/cmake/mujoco.cmake index cb8bd27..ab018fd 100644 --- a/cmake/mujoco.cmake +++ b/cmake/mujoco.cmake @@ -1,14 +1,44 @@ ############################################################################### -## MuJoCo (tarball install, no CMake config shipped) +## MuJoCo: the pinned release, downloaded unless MUJOCO_DIR names an install ############################################################################### -set(MUJOCO_DIR "$ENV{HOME}/.mujoco/mujoco-3.3.6" CACHE PATH "Root of the MuJoCo installation") +set(MUJOCO_VERSION "3.14.0") +set(MUJOCO_SHA256 "326f0da78a7767cc18fab7205c993a771b5f19eda913648f958ee3f625240944") -find_file(MUJOCO_HEADER mujoco/mujoco.h PATHS "${MUJOCO_DIR}/include" NO_DEFAULT_PATH REQUIRED) -find_library(MUJOCO_LIBRARY mujoco PATHS "${MUJOCO_DIR}/lib" NO_DEFAULT_PATH REQUIRED) +set(MUJOCO_DIR "$ENV{MUJOCO_DIR}" CACHE PATH "Root of a MuJoCo installation; empty downloads the pinned release") + +if(MUJOCO_DIR STREQUAL "") + include(FetchContent) + + if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux" OR NOT CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "x86_64|AMD64") + message(FATAL_ERROR "no pinned MuJoCo release for this platform; set MUJOCO_DIR to an installation") + endif() + + FetchContent_Declare(mujoco + URL "https://github.com/google-deepmind/mujoco/releases/download/${MUJOCO_VERSION}/mujoco-${MUJOCO_VERSION}-linux-x86_64.tar.gz" + URL_HASH "SHA256=${MUJOCO_SHA256}" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + ) + FetchContent_MakeAvailable(mujoco) + set(MUJOCO_ROOT "${mujoco_SOURCE_DIR}") +else() + set(MUJOCO_ROOT "${MUJOCO_DIR}") +endif() + +# Found afresh on every configure, so a new pin or MUJOCO_DIR never keeps the old library. +unset(MUJOCO_HEADER CACHE) +unset(MUJOCO_LIBRARY CACHE) +find_file(MUJOCO_HEADER mujoco/mujoco.h PATHS "${MUJOCO_ROOT}/include" NO_DEFAULT_PATH NO_CACHE REQUIRED) +find_library(MUJOCO_LIBRARY mujoco PATHS "${MUJOCO_ROOT}/lib" NO_DEFAULT_PATH NO_CACHE REQUIRED) + +# The release's include directory also carries the ImGui and ImPlot of its own tools, +# newer than the ones the viewer builds with, so only its mujoco/ headers are exposed. +set(MUJOCO_INCLUDE "${PROJECT_BINARY_DIR}/mujoco_include") +file(MAKE_DIRECTORY "${MUJOCO_INCLUDE}") +file(CREATE_LINK "${MUJOCO_ROOT}/include/mujoco" "${MUJOCO_INCLUDE}/mujoco" SYMBOLIC) add_library(mujoco::mujoco SHARED IMPORTED) set_target_properties(mujoco::mujoco PROPERTIES IMPORTED_LOCATION "${MUJOCO_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${MUJOCO_DIR}/include" + INTERFACE_INCLUDE_DIRECTORIES "${MUJOCO_INCLUDE}" ) diff --git a/cmake/templates/run_clang_tidy.sh.in b/cmake/templates/run_clang_tidy.sh.in new file mode 100755 index 0000000..8de0cd4 --- /dev/null +++ b/cmake/templates/run_clang_tidy.sh.in @@ -0,0 +1,59 @@ +#!/bin/bash + +# Positional arguments are the source files to check; they are passed to run-clang-tidy as +# path regexes matched against the entries of the compilation database. +# +# The canonical copy lives in micras-lib; micras-simulation and MicrasFirmware carry the same file +# byte for byte. Each repository configures it with the clang tools it found by their versioned +# names and with the extra arguments its compiler needs (the ARM sysroot on the robot, none on the +# host), one argument per line in MICRAS_TIDY_EXTRA_ARGS. + +CLANG_TIDY="@MICRAS_CLANG_TIDY@" +RUN_CLANG_TIDY="@MICRAS_RUN_CLANG_TIDY@" +CLANG_APPLY_REPLACEMENTS="@MICRAS_CLANG_APPLY_REPLACEMENTS@" + +mapfile -t EXTRA_ARGS <<'EXTRA_ARGS_END' +@MICRAS_TIDY_EXTRA_ARGS@ +EXTRA_ARGS_END + +RUN_FLAGS=(-clang-tidy-binary "${CLANG_TIDY}") + +for ARG in "${EXTRA_ARGS[@]}"; do + if [[ -n "${ARG}" ]]; then + RUN_FLAGS+=("-extra-arg-before=${ARG}") + fi +done + +if [[ "${1:-}" == "--fix" ]]; then + RUN_FLAGS+=(-fix -format -clang-apply-replacements-binary "${CLANG_APPLY_REPLACEMENTS}") + shift +fi + +if [[ -t 1 ]]; then + RUN_FLAGS+=(-use-color) +fi + +# Unknown check names are silently ignored during a normal run, so the configuration is +# validated explicitly to keep it from rotting across clang-tidy versions. +"${CLANG_TIDY}" --verify-config || exit 1 + +echo "Running clang-tidy on all source files..." + +# run-clang-tidy bounds the number of concurrent clang-tidy processes, unlike a plain +# background job per file, and serializes the fixes it applies to shared headers. +# The filtered lines are the echoed clang-tidy invocations and the per-file diagnostic +# counters, which include the warnings suppressed in the vendor headers and are always noise. +# Real diagnostics name their own file, so nothing useful is hidden. +# The turn tables are designed as a constant expression wherever a program compiles them, which +# takes more evaluation steps than clang allows by default. +"${RUN_CLANG_TIDY}" \ + "${RUN_FLAGS[@]}" \ + -quiet \ + -j "$(nproc)" \ + -warnings-as-errors="*" \ + -p "@CMAKE_BINARY_DIR@" \ + -extra-arg-before=-Wno-unknown-warning-option \ + -extra-arg=-fconstexpr-steps=1000000000 \ + "$@" 2>&1 | grep -Ev '^(\[ *[0-9]+/[0-9]+\]|[0-9]+ (warning|error)s? generated\.|clang-tidy(-[0-9.]+)? )' + +exit "${PIPESTATUS[0]}" diff --git a/cmake/third_party.cmake b/cmake/third_party.cmake index 478dd9a..c35cd3c 100644 --- a/cmake/third_party.cmake +++ b/cmake/third_party.cmake @@ -2,31 +2,47 @@ ## Third-party dependencies, pinned and fetched at configure time ############################################################################### -include(FetchContent) +# Each is pinned by commit and fetched as the archive of that commit, checked against its SHA-256: +# a commit cannot be cloned shallow, and a full clone of ImGui alone is 132 MB of history. -set(FETCHCONTENT_BASE_DIR "${CMAKE_SOURCE_DIR}/third_party/_deps" CACHE PATH "FetchContent download directory") +include(FetchContent) FetchContent_Declare(picosha2 - GIT_REPOSITORY https://github.com/okdshin/PicoSHA2.git - GIT_TAG 27fcf6979298949e8a462e16d09a0351c18fcaf2 + URL "https://github.com/okdshin/PicoSHA2/archive/27fcf6979298949e8a462e16d09a0351c18fcaf2.tar.gz" + URL_HASH "SHA256=18d82bb79c021ccf4ce58125b64691accef54237ba5194462740bacf8b39d8a9" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM ) FetchContent_MakeAvailable(picosha2) -if(MICRAS_VIEWER) - # Neither ships a CMakeLists, so both are compiled here from their sources. +# Robot descriptions and scenarios are TOML. v3.4.0. +FetchContent_Declare(tomlplusplus + URL "https://github.com/marzer/tomlplusplus/archive/30172438cee64926dc41fdd9c11fb3ba5b2ba9de.tar.gz" + URL_HASH "SHA256=291254ffe7f2433f90deef878d0d9335534a350a958ea23ecf511b7b2277bf7f" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM +) +FetchContent_MakeAvailable(tomlplusplus) + +if(MICRAS_SIM_VIEWER) + # Neither ships a CMakeLists, so both are compiled here from their sources. ImGui v1.91.5, ImPlot v0.16. FetchContent_Declare(imgui - GIT_REPOSITORY https://github.com/ocornut/imgui.git - GIT_TAG v1.91.5 + URL "https://github.com/ocornut/imgui/archive/f401021d5a5d56fe2304056c391e78f81c8d4b8f.tar.gz" + URL_HASH "SHA256=0a363f5c9dd263233a23087c7f68b3c41a050c9c9b4cf5cf02bf843e672b3464" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM ) FetchContent_MakeAvailable(imgui) FetchContent_Declare(implot - GIT_REPOSITORY https://github.com/epezent/implot.git - GIT_TAG v0.16 + URL "https://github.com/epezent/implot/archive/18c72431f8265e2b0b5378a3a73d8a883b2175ff.tar.gz" + URL_HASH "SHA256=4787c77e6050f3bdc19f39eecf87d5b321bd3096321142b63f8169e1aa8f9b34" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM ) FetchContent_MakeAvailable(implot) - add_library(imgui STATIC + add_library(micras_sim_imgui STATIC "${imgui_SOURCE_DIR}/imgui.cpp" "${imgui_SOURCE_DIR}/imgui_draw.cpp" "${imgui_SOURCE_DIR}/imgui_tables.cpp" @@ -37,7 +53,7 @@ if(MICRAS_VIEWER) "${implot_SOURCE_DIR}/implot_items.cpp" ) - target_include_directories(imgui SYSTEM PUBLIC + target_include_directories(micras_sim_imgui SYSTEM PUBLIC "${imgui_SOURCE_DIR}" "${imgui_SOURCE_DIR}/backends" "${implot_SOURCE_DIR}" @@ -46,30 +62,40 @@ if(MICRAS_VIEWER) find_package(glfw3 QUIET) if(glfw3_FOUND) - target_link_libraries(imgui PUBLIC glfw) + target_link_libraries(micras_sim_imgui PUBLIC glfw) else() find_library(IMGUI_GLFW_LIBRARY NAMES glfw glfw3 REQUIRED) - target_link_libraries(imgui PUBLIC "${IMGUI_GLFW_LIBRARY}") + target_link_libraries(micras_sim_imgui PUBLIC "${IMGUI_GLFW_LIBRARY}") endif() endif() -if(MICRAS_BRIDGE) - set(USE_ZLIB OFF CACHE BOOL "" FORCE) - set(USE_TLS OFF CACHE BOOL "" FORCE) - set(IXWEBSOCKET_INSTALL OFF CACHE BOOL "" FORCE) +if(MICRAS_SIM_BRIDGE) + # Normal variables, which its option() calls honor (CMP0077), so nothing lands in a parent's cache. v11.4.5. + set(USE_ZLIB OFF) + set(USE_TLS OFF) + set(IXWEBSOCKET_INSTALL OFF) FetchContent_Declare(ixwebsocket - GIT_REPOSITORY https://github.com/machinezone/IXWebSocket.git - GIT_TAG v11.4.5 + URL "https://github.com/machinezone/IXWebSocket/archive/c5a02f1066fb0fde48f80f51178429a27f689a39.tar.gz" + URL_HASH "SHA256=ef272693e67daef33275daa8d3685f48e8fe4dbe098338750f9dad3013016d96" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM ) FetchContent_MakeAvailable(ixwebsocket) endif() -if(MICRAS_TESTS) - FetchContent_Declare(googletest - GIT_REPOSITORY https://github.com/google/googletest.git - GIT_TAG b514bdc898e2951020cbdca1304b75f5950d1f59 +if(MICRAS_SIM_TESTS) + # doctest v2.5.3, the commit micras-lib's tests use. Its headers are system headers when it is added + # to another project, so its macros' own code raises no warning here. + set(DOCTEST_NO_INSTALL ON) + + FetchContent_Declare(doctest + URL "https://github.com/doctest/doctest/archive/2d0a9359a60c51affe2a9bebb1be1dca47868151.tar.gz" + URL_HASH "SHA256=e64542c4ea68e9f381ccf6eae924cfdd652567c87c142d76fe92644fb4608149" + DOWNLOAD_EXTRACT_TIMESTAMP TRUE + SYSTEM ) - set(INSTALL_GTEST OFF CACHE BOOL "" FORCE) - FetchContent_MakeAvailable(googletest) + FetchContent_MakeAvailable(doctest) + + include("${doctest_SOURCE_DIR}/scripts/cmake/doctest.cmake") endif() diff --git a/config/constants.hpp b/config/constants.hpp deleted file mode 100644 index 08556eb..0000000 --- a/config/constants.hpp +++ /dev/null @@ -1,230 +0,0 @@ -/** - * @file - * - * @brief Navigation constants for the ROS-simulation baseline. - * - * @note This is a copy of MicrasFirmware/config/constants.hpp that - * INTENTIONALLY diverges from it. The values below reproduce the tuning - * the ROS simulation was validated against, not the values flashed on the - * real robot, so the two files are expected to differ. Every intentional - * divergence is listed here and mirrored in the allowlist of - * tools/check_config_drift.py; anything else showing up in that script is - * accidental drift and must be fixed rather than allowlisted. - * - * - crash_acceleration: 1e6 instead of 35, which disables crash detection - * altogether. The simulated chassis sees impulses the real one never - * does and would abort the run on contact with a wall. - * - speed_controller linear_pid: kp = 120, ki = 40 instead of kp = 10, - * ki = 1; angular_pid: kp = 16, ki = 10 instead of kp = 2, ki = 1. - * Same structure as the firmware, rescaled to MuJoCo command units. - * The PID response is summed straight onto the feed-forward command, - * so kp scales with the feed-forward gain of its own axis: linear - * 102 / 12.706 = 8.0 times the firmware kp, angular 4.9 / 0.971 = 5.0 - * times it, rounded to 120 and 16. ki is 10 instead of 1 because a - * 180 deg pivot only lasts 1.6 s: at ki = 1 the integral had closed - * less than half of the 4 percent steady-state angular error by the - * end of the turn, and since TurnAction is open loop in time the - * missing area came out as a 21 deg heading error per turn. With - * ki = 10 the executed turn angle is 98.7 percent of the commanded - * one. ki is 40 on the linear axis rather than 10 because - * Micras::run calls speed_controller.reset() at every action boundary - * and an exploring action is only 45 to 180 mm long, so the integral - * gets 0.16 to 0.6 s to close the 5 percent steady-state speed error - * before being wiped. That error matters because TurnAction holds a - * constant linear speed for a fixed time: at -5.5 percent the exploring - * curve displaces 41.7 mm instead of the 45 mm the ActionQueuer - * geometry assumes, and MoveAction measures distance travelled rather - * than position in the cell, so the missing millimetres never come - * back. ki = 40 took the first ERROR of a 120 s explore from 22.3 s to - * 29.0 s and the maximum odometry error from 131 to 57 mm. - * BEWARE: the firmware PID evaluates kp * (error + ki * integral - - * kd * derivative), so kp = 0 would make the whole PID output - * identically zero. That is what this file used to do. - * - action_queuer exploring max_centrifugal_acceleration: 2.0 instead of - * 2.78, and max_linear_speed 0.3 instead of 0.4. The exploring curve - * radius is max_linear_speed^2 / max_centrifugal_acceleration, so the - * pair that used to be here (0.3 and 1.0) asked for a 90 mm radius. - * The corridor is 180 - 12.6 = 167.4 mm wide and the chassis is 66 mm - * wide with a 66 mm front overhang, so a 90 mm curve puts the outer - * front corner into the wall: the robot wedged for 8.4 s of a 120 s - * explore. 0.3^2 / 2.0 = 45 mm clears it and cuts the wedged time to - * 1.3 s and the chassis contact fraction from 0.074 to 0.013. - * - left/right feed_forward linear_speed: 102 instead of 12.706/13.319. - * Calibrated to the MuJoCo motor, whose no-load speed is 1.01 m/s at - * ctrl = 100, rather than to the real motor and gearbox. - * - follow_wall pid saturation: 4.0 instead of 0.5, and kd 0.05 instead of - * 0.0. FollowWall returns state.velocity.linear * pid(error), where the - * error is the left minus right rangefinder reading; around the centre - * of the corridor that error is 5.34 per metre of lateral offset, so - * with kp = 30 the proportional term alone reaches the old saturation - * of 1.0 at 6.2 mm of offset. Beyond that the lateral loop was pure - * bang-bang at +-v rad/s and limit-cycled: the robot crossed the centre - * line with the full heading error still on it, entered the next curve - * 8 deg off and walked into the wall. Raising the saturation to 4.0 - * keeps the loop proportional out to 25 mm, a full half corridor. - * The undamped loop has omega_n = v * sqrt(kp * 5.34) = 3.4 rad/s, a - * 1.9 s period, i.e. half a metre of travel per swing; kd adds - * zeta = v * sqrt(kp * 5.34) * kd / 2, so 0.05 is the largest value - * that damps it without letting the derivative kick on the step the - * reading takes when a wall ends. Measured over a 120 s explore: - * lateral rms 25.0 -> 8.7 mm, chassis contact 0.0027 -> 0.0049, first - * ERROR 29.0 -> 98.2 s and the goal reached at 38.8 s instead of never. - * - follow_wall pid kp: 30 instead of 0.5, and post_threshold 400 instead - * of 16.5, because the simulated rangefinders return metres over a very - * different scale than the real ADC readings. - * - odometry linear_cutoff_frequency: 15.27 instead of 7.64, since the - * simulated encoders carry no noise to filter out. Both numbers are - * 1.527 times the pre-2026-09 ones because the ButterworthFilter was - * normalising by f_c instead of 2 * pi * f_c and defaulting to a 100 Hz - * sampling rate while running at loop_frequency; every cutoff was - * restated as the frequency actually in effect, so nothing changed but - * the label. - * - wall_sensors filter_cutoff: 30.55 instead of 7.64, because the - * simulated rangefinders are noise free and the post detection in - * FollowWall::check_posts needs the sharpest edge available. - */ - -#ifndef MICRAS_CONSTANTS_HPP -#define MICRAS_CONSTANTS_HPP - -#include -#include - -#include "micras/nav/action_queuer.hpp" -#include "micras/nav/follow_wall.hpp" -#include "micras/nav/maze.hpp" -#include "micras/nav/odometry.hpp" -#include "micras/nav/speed_controller.hpp" - -namespace micras { -/*************** - * Constants - ***************/ - -constexpr bool debug_mode{false}; -constexpr uint8_t maze_width{16}; -constexpr uint8_t maze_height{16}; -constexpr float cell_size{0.18}; -constexpr uint32_t loop_time_us{1042}; -constexpr float loop_frequency{1000000.0F / loop_time_us}; -constexpr float wall_thickness{0.012F}; -constexpr float start_offset{0.05F}; -constexpr float max_angular_acceleration{400.0F}; -constexpr float crash_acceleration{1000000.0F}; -constexpr float fan_speed{100.0F}; - -constexpr core::WallSensorsIndex wall_sensors_index{ - .left_front = 0, - .left = 1, - .right = 2, - .right_front = 3, -}; - -/*************** - * Template Instantiations - ***************/ - -namespace nav { -using Maze = TMaze; -} // namespace nav - -/*************** - * Configurations - ***************/ - -const nav::ActionQueuer::Config action_queuer_config{ - .cell_size = cell_size, - .start_offset = start_offset, - .curve_safety_margin = 0.053F, - .exploring = - { - .max_linear_speed = 0.3F, - .max_linear_acceleration = 5.0F, - .max_linear_deceleration = 5.0F, - .max_centrifugal_acceleration = 2.0F, - .max_angular_acceleration = 200.0F, - }, - .solving = - { - .max_linear_speed = 5.0F, - .max_linear_acceleration = 12.0F, - .max_linear_deceleration = 20.0F, - .max_centrifugal_acceleration = 40.0F, - .max_angular_acceleration = max_angular_acceleration, - }, -}; - -const nav::FollowWall::Config follow_wall_config{ - .pid = - { - .kp = 30.0F, - .ki = 0.0F, - .kd = 0.05F, - .setpoint = 0.0F, - .saturation = 4.0F, - .max_integral = -1.0F, - }, - .wall_sensor_index = wall_sensors_index, - .max_angular_acceleration = max_angular_acceleration, - .cell_size = cell_size, - .post_threshold = 400.0F, - .post_reference = 0.44F * cell_size, - .post_clearance = 0.035F, -}; - -const nav::Maze::Config maze_config{ - .start = {{0, 0}, nav::Side::UP}, - .goal = {{ - {maze_width / 2, maze_height / 2}, - {(maze_width - 1) / 2, maze_height / 2}, - {maze_width / 2, (maze_height - 1) / 2}, - {(maze_width - 1) / 2, (maze_height - 1) / 2}, - }}, - .cost_margin = 1.2F, - .action_queuer_config = action_queuer_config, -}; - -const nav::Odometry::Config odometry_config{ - .linear_cutoff_frequency = 15.27F, - .sampling_frequency = loop_frequency, - .wheel_radius = 0.011F, - .initial_pose = {{cell_size / 2.0F, start_offset}, std::numbers::pi_v / 2.0F}, -}; - -const nav::SpeedController::Config speed_controller_config{ - .linear_pid = - { - .kp = 120.0F, - .ki = 40.0F, - .kd = 0.0F, - .setpoint = 0.0F, - .saturation = 40.0F, - .max_integral = -1.0F, - }, - .angular_pid = - { - .kp = 16.0F, - .ki = 10.0F, - .kd = 0.0F, - .setpoint = 0.0F, - .saturation = 40.0F, - .max_integral = -1.0F, - }, - .left_feed_forward = - { - .linear_speed = 102.0F, - .linear_acceleration = 0.3F, - .angular_speed = -4.9F, - .angular_acceleration = -0.1F, - }, - .right_feed_forward = - { - .linear_speed = 102.0F, - .linear_acceleration = 0.3F, - .angular_speed = +4.9F, - .angular_acceleration = +0.1F, - }, -}; -} // namespace micras - -#endif // MICRAS_CONSTANTS_HPP diff --git a/config/target.hpp b/config/target.hpp deleted file mode 100644 index f8aa2fd..0000000 --- a/config/target.hpp +++ /dev/null @@ -1,180 +0,0 @@ -/** - * @file target.hpp - * - * @brief Target specific configuration for the MuJoCo harness - * - * @note Every name below is resolved against the loaded MuJoCo model when the - * proxy is constructed. This is the one file that names - * sim::SimulationContext::instance(): every proxy receives the facet it - * needs through its Config, so nothing else has to know the context is - * process-wide. - * - * @date 03/2024 - */ - -#ifndef MICRAS_TARGET_HPP -#define MICRAS_TARGET_HPP - -#include "constants.hpp" -#include "micras/proxy/argb.hpp" -#include "micras/proxy/battery.hpp" -#include "micras/proxy/bluetooth_serial.hpp" -#include "micras/proxy/button.hpp" -#include "micras/proxy/buzzer.hpp" -#include "micras/proxy/dip_switch.hpp" -#include "micras/proxy/fan.hpp" -#include "micras/proxy/imu.hpp" -#include "micras/proxy/led.hpp" -#include "micras/proxy/locomotion.hpp" -#include "micras/proxy/rotary_sensor.hpp" -#include "micras/proxy/storage.hpp" -#include "micras/proxy/torque_sensors.hpp" -#include "micras/proxy/wall_sensors.hpp" -#include "micras/sim/core/simulation_context.hpp" - -// clang-format off -namespace micras { -/***************************************** - * Template Instantiations - *****************************************/ - -namespace proxy { - using Argb = proxy::TArgb<2>; - using DipSwitch = TDipSwitch<4>; - using TorqueSensors = TTorqueSensors<2>; - using WallSensors = TWallSensors<4>; -} // namespace proxy - -const proxy::Stopwatch::Config stopwatch_config { - .context = &sim::SimulationContext::instance() -}; - -const proxy::Storage::Config maze_storage_config { - .start_page = 2, - .number_of_pages = 1 -}; - -/***************************************** - * Interface - *****************************************/ - -const proxy::Led::Config led_config { - .name = "led", - .context = &sim::SimulationContext::instance() -}; - -const proxy::Argb::Config argb_config { - .names = { - "rgb_0", - "rgb_1" - }, - .context = &sim::SimulationContext::instance() -}; - -const proxy::Button::Config button_config { - .name = "button", - .long_press_delay = 500U, - .extra_long_press_delay = 2000U, - .context = &sim::SimulationContext::instance() -}; - -const proxy::DipSwitch::Config dip_switch_config { - .names = { - "dip_switch_0", - "dip_switch_1", - "dip_switch_2", - "dip_switch_3" - }, - .context = &sim::SimulationContext::instance() -}; - -const proxy::Buzzer::Config buzzer_config { - .name = "buzzer", - .context = &sim::SimulationContext::instance() -}; - -const proxy::BluetoothSerial::Config bluetooth_config { - .context = &sim::SimulationContext::instance() -}; - -/***************************************** - * Sensors - *****************************************/ - -const proxy::RotarySensor::Config rotary_sensor_left_config { - .sensor = "encoder_left", - .wheel = 0, - .context = &sim::SimulationContext::instance() -}; - -const proxy::RotarySensor::Config rotary_sensor_right_config { - .sensor = "encoder_right", - .wheel = 1, - .context = &sim::SimulationContext::instance() -}; - -const proxy::TorqueSensors::Config torque_sensors_config { - .shunt_resistor = 0.04F * 20, - .max_torque = 3.0F, - .reference_voltage = 3.3F -}; - -const proxy::WallSensors::Config wall_sensors_config { - .sensors = { - "lidar_0", - "lidar_1", - "lidar_2", - "lidar_3" - }, - .uncertainty = 0.5F, - .base_readings = { - 0.0696F, - 0.1090F, - 0.1090F, - 0.0696F, - }, - .max_sensor_reading = 0.6F, - .min_sensor_reading = 0.01F, - .max_sensor_distance = 0.18F * 2, - .filter_cutoff = 30.55F, - .sampling_frequency = loop_frequency, - .context = &sim::SimulationContext::instance() -}; - -const proxy::Imu::Config imu_config { - .gyro_sensor = "gyro", - .accelerometer_sensor = "accelerometer", - .context = &sim::SimulationContext::instance() -}; - -const proxy::Battery::Config battery_config { - .max_voltage = 9.9F, - .max_reading = 4095 -}; - -/***************************************** - * Actuators - *****************************************/ - -const proxy::Fan::Config fan_config { - .actuator = "fan", - .max_acceleration = 0.02F, - .context = &sim::SimulationContext::instance() -}; - -const proxy::Locomotion::Config locomotion_config { - .left_motor = { - .actuator = "motor_left", - .context = &sim::SimulationContext::instance() - }, - .right_motor = { - .actuator = "motor_right", - .context = &sim::SimulationContext::instance() - }, - .context = &sim::SimulationContext::instance() -}; -} // namespace micras - -// clang-format on - -#endif // MICRAS_TARGET_HPP diff --git a/engine/arenas/maze/include/micras/sim/arenas/maze.hpp b/engine/arenas/maze/include/micras/sim/arenas/maze.hpp new file mode 100644 index 0000000..ca2bf13 --- /dev/null +++ b/engine/arenas/maze/include/micras/sim/arenas/maze.hpp @@ -0,0 +1,159 @@ +/** + * @file + * + * @brief A micromouse maze, read from the usual ASCII drawing, as an arena. + */ + +#ifndef MICRAS_SIM_ARENAS_MAZE_HPP +#define MICRAS_SIM_ARENAS_MAZE_HPP + +#include +#include +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief Dimensions and surfaces of a maze. + * + * @note The defaults are the competition rules: 180 mm cells, 12 mm thick and + * 50 mm tall walls. The reflectances are what an infrared range sensor sees: + * white wall sides, a black floor. + */ +struct MazeConfig { + double cell_size{0.18}; + double wall_thickness{0.012}; + double wall_height{0.05}; + double wall_reflectance{0.8}; + double floor_reflectance{0.05}; +}; + +/** + * @brief A maze: which walls stand, where it starts and where the goal is. + * + * @note The drawing is the format of the published maze collections: posts as + * 'o', horizontal walls as '---', vertical walls as '|', the start cell + * marked 'S' and goal cells 'G', read bottom-up so the start is at the + * lower left. The world frame puts the lower left post at + * the origin, x to the right and y up; a cell (x, y) spans + * [x, x + 1) and [y, y + 1) cells. + */ +class Maze { +public: + /** + * @brief Name of the arena body, which prefixes every geom once attached. + */ + static constexpr std::string_view body_name{"maze"}; + + /** + * @brief Read a maze drawing from a file. + * + * @param path Path of the drawing. + * @return The maze. + */ + static Maze load(const std::filesystem::path& path); + + /** + * @brief Read a maze drawing. + * + * @param text The drawing. + * @return The maze. + */ + static Maze parse(std::string_view text); + + /** + * @brief Get the size of the maze in cells. + * + * @return Width and height. + */ + ///@{ + std::size_t width() const { return this->columns; } + + std::size_t height() const { return this->rows; } + + ///@} + + /** + * @brief Check for a wall on the south edge of a cell, which row may be one past the top. + * + * @param column Column of the cell. + * @param row Row of the cell. + * @return True when the wall stands. + */ + bool south_wall(std::size_t column, std::size_t row) const; + + /** + * @brief Check for a wall on the west edge of a cell, which column may be one past the right. + * + * @param column Column of the cell. + * @param row Row of the cell. + * @return True when the wall stands. + */ + bool west_wall(std::size_t column, std::size_t row) const; + + /** + * @brief Get the start cell. + * + * @return Its column and row; the lower left cell when the drawing marks none. + */ + std::pair start() const { return this->start_cell; } + + /** + * @brief Get the goal cells. + * + * @return Their columns and rows. + */ + const std::vector>& goals() const { return this->goal_cells; } + + /** + * @brief Generate the arena's MJCF: a body holding the floor, the posts and the walls. + * + * @note The walls and posts are white with a red top, the usual colors, except around the start + * and the goal cells, which are all white. The floor is a checker of cells, on the middle of + * the walls. The red tops are paint: they are in the paint group and collide with nothing, + * so none of that touches what the robot and its sensors meet. + * + * @param config Dimensions and surfaces. + * @return The MJCF text. + */ + std::string mjcf(const MazeConfig& config) const; + + /** + * @brief Get the infrared reflectance of an arena geom. + * + * @param geom_name Name of the geom, with or without the attach prefix. + * @param config Dimensions and surfaces. + * @return The reflectance. + */ + static double reflectance(std::string_view geom_name, const MazeConfig& config); + +private: + /** + * @brief Check if a cell is the start or one of the goal cells, whose walls are all white. + * + * @param column Column of the cell. + * @param row Row of the cell. + * @return True if the cell is marked. + */ + bool is_marked(std::size_t column, std::size_t row) const; + + /** + * @brief Generate the red paint on top of every post and wall, but those around the marked cells. + * + * @param config Dimensions and surfaces. + * @return The MJCF geoms of the paint. + */ + std::string paint(const MazeConfig& config) const; + + std::size_t columns{0}; + std::size_t rows{0}; + std::vector> south; + std::vector> west; + std::pair start_cell{0, 0}; + std::vector> goal_cells; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_ARENAS_MAZE_HPP diff --git a/engine/arenas/maze/mazes/alljapan-033-2012-exp-fin.txt b/engine/arenas/maze/mazes/alljapan-033-2012-exp-fin.txt new file mode 100644 index 0000000..6b41392 --- /dev/null +++ b/engine/arenas/maze/mazes/alljapan-033-2012-exp-fin.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | | +o o o o---o---o---o o---o---o---o---o o o o o o +| | | | | | | | | | | +o o o o o o o---o o---o---o---o---o o o o o +| | | | | | | | | +o o---o---o o o---o---o---o---o---o---o o o---o---o o +| | | | | | +o o---o---o---o o o---o---o---o---o---o o---o---o o o +| | | | | | | | +o o o---o o---o o o o---o o o---o o---o o o +| | | | | | | | | | | | +o o---o o---o o o o o o o---o o o o---o o +| | | | | | | | | | +o o o---o---o o o o o---o o o---o o---o o o +| | | | | G G | | | | | +o o---o o---o o o o o o o o o---o o---o o +| | | | | G G | | | | | +o o o---o o o o o o---o o o---o o---o---o o +| | | | | | | | | | +o o---o o---o o o o o o---o---o o---o---o o o +| | | | | | | | +o---o---o---o---o o o o o o o---o o---o o o o +| | | | | | | | | +o o---o---o---o o o o o o o---o---o o---o o o +| | | | | | | | | +o---o---o o---o o---o---o o---o o o o o o---o o +| | | | | | | | | +o o o o o o---o o---o---o---o o o o o---o---o +| | | | | | | | | +o o o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/apec2016.txt b/engine/arenas/maze/mazes/apec2016.txt new file mode 100644 index 0000000..0b171b8 --- /dev/null +++ b/engine/arenas/maze/mazes/apec2016.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o o o o o---o---o---o o o +| | | | | | | | | | | | | +o o o o o---o---o---o o o o o---o o o---o o +| | | | | | | | | | +o o o o o---o---o---o o o o o---o o---o o o +| | | | | | | | | | | | | | +o o o o o o o o---o o---o---o o---o o o o +| | | | | | | | | | +o o o o---o---o o o---o---o---o o---o---o o o o +| | | | | | G G | | | | | | +o o o o o o---o o o o o---o---o o o o o +| | | | | | | G G | | | | +o o o o o---o o o---o---o---o---o o o o o o +| | | | | | | | | | | | | +o o o o o o---o---o---o---o o o o o---o---o o +| | | | | | | | | | | +o o o o o o o o---o o---o o o---o---o o o +| | | | | | | | | | | +o o o o---o---o o---o o---o o o---o---o o o o +| | | | | | | | | | +o o---o o o---o---o o---o o---o o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/apec2017.txt b/engine/arenas/maze/mazes/apec2017.txt new file mode 100644 index 0000000..b03869e --- /dev/null +++ b/engine/arenas/maze/mazes/apec2017.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | +o o o---o---o o o o o---o---o o o---o o o o +| | | | | | | | | | | | +o o o o o---o o---o---o o o o---o o o o o +| | | | | | | | | | | | | | +o o o o---o o---o o o---o o o o---o o o o +| | | | | | | | | | | | | +o o o o o---o o o---o o o o---o o o o o +| | | | | | | | | | | | +o o o o o o o---o---o---o---o---o---o o o o o +| | | | | G G | | | | | +o o o---o---o o---o o o o o---o o o o o o +| | | | G G | | | | | | +o o o o---o---o---o---o---o o---o o o---o o o o +| | | | | | | | | | +o o o---o o o---o o---o---o o o---o o---o o o +| | | | | | | | | | | | +o o o o---o o o---o---o o o---o o---o o o o +| | | | | | | | | | | | | +o o o---o o o---o o o---o---o o---o o o o o +| | | | | | | | | | | +o o---o o o---o o o---o o o---o o---o o o o +| | | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/apec2018.txt b/engine/arenas/maze/mazes/apec2018.txt new file mode 100644 index 0000000..f38c3ef --- /dev/null +++ b/engine/arenas/maze/mazes/apec2018.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | +o o o o o---o---o o---o---o---o---o---o---o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o o o o o o o o +| | | | | | | | | | | +o o o o---o o---o o---o---o---o o o o o---o o +| | | | | | | | | | +o o o o o o---o---o---o---o---o o o o---o o o +| | | | | | | | | | | | | +o o o o o---o o o---o---o o o o o o---o o +| | | | | | | G G | | | | | | | +o o o o---o o o o o o o o o o---o o o +| | | | | G G | | | | | | +o o o o o---o o o---o---o o o o---o o o o +| | | | | | | | | | | +o o o o---o o o---o---o o---o---o o o---o o o +| | | | | | | | | | | | | +o o o o o o---o o o---o o o---o---o o---o o +| | | | | | | | | | | | +o o o o o---o o o---o o---o o---o---o---o o o +| | | | | | | | | | +o o o o---o o---o---o---o---o---o o o o o o o +| | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/apec2019.txt b/engine/arenas/maze/mazes/apec2019.txt new file mode 100644 index 0000000..02cf47b --- /dev/null +++ b/engine/arenas/maze/mazes/apec2019.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| | | | | | | | +o o o o---o o---o o---o o---o o---o o---o o o +| | | | | | | | | | | +o o o o o---o o---o o---o o---o o---o o o o +| | | | | | | | | | | +o o o o---o o---o o---o o---o o---o---o---o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o o o---o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o---o---o o---o o +| | | | | | G G | | | +o o o o o o---o---o o o o---o---o---o---o o o +| | | | | | G G | | | | +o o o o o---o---o o---o---o---o---o o o---o---o o +| | | | | | | | | +o o o o---o o o---o---o---o o o---o---o---o o o +| | | | | | | | | | +o o o o o---o o o---o o---o---o o---o---o o o +| | | | | | | | | | +o o o o---o---o o---o o---o---o o---o---o o o o +| | | | | | | | | | | +o o o o o---o---o---o o o---o---o o o o o o +| | | | | | | | +o o o o---o---o---o---o---o---o---o---o---o---o---o o o +| | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/japan2013ef.txt b/engine/arenas/maze/mazes/japan2013ef.txt new file mode 100644 index 0000000..05d8df4 --- /dev/null +++ b/engine/arenas/maze/mazes/japan2013ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | | +o o---o o---o---o---o---o---o---o---o---o---o o o o o +| | | | | | | +o o---o---o o o---o---o---o---o---o---o o---o o o o +| | | | | | | | +o o---o o---o o o---o o---o o---o---o o---o---o o +| | | | | | | | | +o o---o o---o---o o---o o---o o o o o---o o o +| | | | | | | | | | +o o---o o---o o---o---o o---o---o---o o o---o o o +| | | | | | | | | | +o o o---o o---o o---o o---o o o o---o o o o +| | | | | | | | | | +o o o---o---o o o---o o---o---o o---o o o o o +| | | | | G G | | | | | | | +o o---o---o o o---o o o o o---o o o o o o +| | | | | G G | | | | | | +o o o---o---o o o---o---o---o---o o---o o o o o +| | | | | | | | | | +o o---o---o o---o---o o o o o---o o---o o o o +| | | | | | | | | | | +o o o---o---o o o---o---o---o---o o o---o o o o +| | | | | | | | | +o o o---o---o---o o o o---o o---o o o---o o o +| | | | | | | | | | | +o o o o o o---o---o---o---o o o---o o o o o +| | | | | | | | | | +o o o---o---o o o---o o o---o---o o o o---o o +| | | | | | | | | | +o o o o o---o---o---o---o---o---o---o---o---o o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/mazes/japan2017ef.txt b/engine/arenas/maze/mazes/japan2017ef.txt new file mode 100644 index 0000000..42b70e0 --- /dev/null +++ b/engine/arenas/maze/mazes/japan2017ef.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | | +o o o o---o---o---o---o---o---o---o---o---o---o o o---o +| | | | | | | | +o o---o o o---o---o o o---o---o o o o---o---o o +| | | | | | | | | +o o---o o o o o---o---o o o---o---o---o---o o o +| | | | | | | +o o---o---o---o---o---o---o---o---o---o---o---o o o---o o +| | | | | | | | | +o o o o o o o o o o o o o---o---o o o +| | | | | | | | | | | +o o o---o---o---o---o---o---o---o---o---o---o---o o---o o +| | | | | | | | +o o---o o o o---o o o---o o o o---o o---o o +| | | | | | G G | | | | +o o o---o o---o o o o o---o---o---o---o o---o o +| | | | | | G G | | | | +o o o o---o o---o o---o---o o o o---o---o---o o +| | | | | | | +o o---o---o o---o---o---o---o---o---o---o---o---o---o o o +| | | | | | | | | | +o o o---o---o o o o o o o o o---o o---o o +| | | | | | | | | | +o o---o o o---o---o o---o---o o---o o o---o o o +| | | | | | | | | | | +o o o---o---o o o---o o o---o o---o o---o---o o +| | | | | | | | | +o o o o---o---o o---o---o---o---o---o o o o o o +| | | | | | | | | +o o---o o o o---o---o---o---o---o---o---o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/models/maze.txt b/engine/arenas/maze/mazes/maze1.txt similarity index 100% rename from models/maze.txt rename to engine/arenas/maze/mazes/maze1.txt diff --git a/models/maze2.txt b/engine/arenas/maze/mazes/maze2.txt similarity index 100% rename from models/maze2.txt rename to engine/arenas/maze/mazes/maze2.txt diff --git a/engine/arenas/maze/mazes/uk2016f.txt b/engine/arenas/maze/mazes/uk2016f.txt new file mode 100644 index 0000000..e1123b0 --- /dev/null +++ b/engine/arenas/maze/mazes/uk2016f.txt @@ -0,0 +1,33 @@ +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o +| | +o o---o---o---o---o---o o---o---o---o o---o---o---o---o o +| | | | +o---o o o o---o---o---o---o---o o---o o---o o o o +| | | | | | | +o o o o---o o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o o o---o---o---o---o---o o o o o o +| | | | | | | | +o o---o---o o---o o o---o---o---o o---o o o o o +| | | | | | | | | | +o o o o---o o o o---o o o o o o o o o +| | | | | | | | | | | | | +o o o---o o o o o---o---o o o o o---o o o +| | | | | G G | | | | | | | +o o---o o---o---o---o o o o o o o o o---o o +| | | | G G | | | | | +o o o o o---o---o o---o---o---o o o o o o---o +| | | | | | | | | +o o o o---o---o---o---o o---o---o---o o o o o o +| | | | | | | | | +o o o o o---o---o o---o---o---o o o o o o o +| | | | | | | | | | +o o---o o o o o---o---o o---o---o o o o o o +| | | | | | | | +o o o---o o o---o o---o---o---o---o---o o o o o +| | | | | | | +o o---o o o---o---o---o---o---o o o---o---o---o o---o +| | | | | | +o o o---o---o o---o---o---o o---o---o o---o---o---o o +| S | | +o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o---o diff --git a/engine/arenas/maze/src/maze.cpp b/engine/arenas/maze/src/maze.cpp new file mode 100644 index 0000000..0cc7522 --- /dev/null +++ b/engine/arenas/maze/src/maze.cpp @@ -0,0 +1,267 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/arenas/maze.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/text_file.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Characters one cell takes in a line of the drawing. + */ +constexpr std::size_t cell_width{4}; +} // namespace + +/** + * @brief Split a drawing into lines, dropping trailing spaces and blank lines at the end. + * + * @param text The drawing. + * @return Its lines, top first. + */ +static std::vector lines_of(std::string_view text) { + std::vector lines; + std::istringstream stream{std::string{text}}; + std::string line; + + while (std::getline(stream, line)) { + while (not line.empty() and (line.back() == ' ' or line.back() == '\r')) { + line.pop_back(); + } + + lines.push_back(line); + } + + while (not lines.empty() and lines.back().empty()) { + lines.pop_back(); + } + + return lines; +} + +/** + * @brief Get a character of a line, or a space past its end. + * + * @param line The line. + * @param index Index of the character. + * @return The character. + */ +static char at(const std::string& line, std::size_t index) { + return index < line.size() ? line.at(index) : ' '; +} + +namespace { +/** + * @brief Half the thickness of the red paint on top of the walls and posts, in meters. + * + * @note The paint is a box of its own that collides with nothing, so the walls the sensors and the + * robot meet are the same whatever their color. + */ +constexpr double cap_half_height{0.00025}; + +/** + * @brief Number of cells the floor reaches past the maze on every side. + * + * @note MuJoCo lays the texture of a plane from its corner, so a whole number of cells keeps the + * checker on the middle of the walls. + */ +constexpr double floor_margin{2.0}; +} // namespace + +Maze Maze::load(const std::filesystem::path& path) { + return parse(read_text_file(path, "maze")); +} + +Maze Maze::parse(std::string_view text) { + std::vector lines = lines_of(text); + + if (lines.size() < 3 or lines.size() % 2 == 0 or lines.front().size() < cell_width + 1) { + throw std::runtime_error("a maze drawing needs an odd number of lines, alternating posts and cells"); + } + + std::ranges::reverse(lines); + + Maze maze; + maze.columns = (lines.front().size() - 1) / cell_width; + maze.rows = (lines.size() - 1) / 2; + maze.south.assign(maze.rows + 1, std::vector(maze.columns, false)); + maze.west.assign(maze.rows, std::vector(maze.columns + 1, false)); + + for (std::size_t row = 0; row <= maze.rows; row++) { + for (std::size_t column = 0; column < maze.columns; column++) { + maze.south.at(row).at(column) = + lines.at(2 * row).substr(std::min(lines.at(2 * row).size(), cell_width * column + 1), 3) == "---"; + } + } + + for (std::size_t row = 0; row < maze.rows; row++) { + const std::string& line = lines.at(2 * row + 1); + + for (std::size_t column = 0; column <= maze.columns; column++) { + maze.west.at(row).at(column) = at(line, cell_width * column) == '|'; + } + + for (std::size_t column = 0; column < maze.columns; column++) { + const char mark = at(line, cell_width * column + 2); + + if (mark == 'S') { + maze.start_cell = {column, row}; + } else if (mark == 'G') { + maze.goal_cells.emplace_back(column, row); + } + } + } + + return maze; +} + +bool Maze::south_wall(std::size_t column, std::size_t row) const { + return row < this->south.size() and column < this->columns and this->south.at(row).at(column); +} + +bool Maze::west_wall(std::size_t column, std::size_t row) const { + return row < this->rows and column < this->west.at(row).size() and this->west.at(row).at(column); +} + +double Maze::reflectance(std::string_view geom_name, const MazeConfig& config) { + return geom_name.ends_with("floor") ? config.floor_reflectance : config.wall_reflectance; +} + +bool Maze::is_marked(std::size_t column, std::size_t row) const { + const std::pair cell{column, row}; + return cell == this->start_cell or std::ranges::find(this->goal_cells, cell) != this->goal_cells.end(); +} + +std::string Maze::paint(const MazeConfig& config) const { + const double cell = config.cell_size; + const double half_thickness = config.wall_thickness / 2; + const double half_length = (cell - config.wall_thickness) / 2; + const double z = config.wall_height + cap_half_height; + std::string caps; + + const auto cap = [&caps, + z](std::string_view name, std::size_t column, std::size_t row, double x, double y, double half_x, + double half_y) { + caps += std::format( + " \n", + name, column, row, x, y, z, half_x, half_y, cap_half_height + ); + }; + + for (std::size_t row = 0; row <= this->rows; row++) { + for (std::size_t column = 0; column <= this->columns; column++) { + const double x = static_cast(column) * cell; + const double y = static_cast(row) * cell; + const bool left = column > 0 and this->is_marked(column - 1, row); + const bool below = row > 0 and this->is_marked(column, row - 1); + const bool here = this->is_marked(column, row); + const bool corner = column > 0 and row > 0 and this->is_marked(column - 1, row - 1); + + if (not(here or left or below or corner)) { + cap("post", column, row, x, y, half_thickness, half_thickness); + } + + if (this->south_wall(column, row) and not(here or below)) { + cap("south", column, row, x + cell / 2, y, half_length, half_thickness); + } + + if (this->west_wall(column, row) and not(here or left)) { + cap("west", column, row, x, y + cell / 2, half_thickness, half_length); + } + } + } + + return caps; +} + +std::string Maze::mjcf(const MazeConfig& config) const { + const double cell = config.cell_size; + const double half_thickness = config.wall_thickness / 2; + const double half_height = config.wall_height / 2; + const double half_length = (cell - config.wall_thickness) / 2; + const double width = static_cast(this->columns) * cell; + const double height = static_cast(this->rows) * cell; + std::string geoms; + + for (std::size_t row = 0; row <= this->rows; row++) { + for (std::size_t column = 0; column <= this->columns; column++) { + geoms += std::format( + " \n", column, row, + static_cast(column) * cell, static_cast(row) * cell, half_height + ); + + if (this->south_wall(column, row)) { + geoms += std::format( + " \n", column, + row, (static_cast(column) + 0.5) * cell, static_cast(row) * cell, half_height + ); + } + + if (this->west_wall(column, row)) { + geoms += std::format( + " \n", column, row, + static_cast(column) * cell, (static_cast(row) + 0.5) * cell, half_height + ); + } + } + } + + const std::string caps = this->paint(config); + + return std::format( + "\n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + " \n" + "{10}{11}" + " \n" + " \n" + "\n", + 1.0 / cell, half_thickness, half_height, half_length, body_name, width / 2, height / 2, + std::max(width, height) + 0.2, width / 2 + floor_margin * cell, height / 2 + floor_margin * cell, geoms, caps, + MujocoWorld::unseen_group + ); +} +} // namespace micras::sim diff --git a/include/micras/sim/core/clock.hpp b/engine/include/micras/sim/core/clock.hpp similarity index 77% rename from include/micras/sim/core/clock.hpp rename to engine/include/micras/sim/core/clock.hpp index 95a3a46..d05c44a 100644 --- a/include/micras/sim/core/clock.hpp +++ b/engine/include/micras/sim/core/clock.hpp @@ -36,11 +36,6 @@ class Clock { */ void advance(); - /** - * @brief Rewind to the start of a run. - */ - void reset(); - /** * @brief Get the current simulated time. * @@ -69,18 +64,6 @@ class Clock { */ int steps_per_tick() const { return this->tick_steps; } - /** - * @brief Get how many ticks a duration spans. - * - * @note Also available before a model is loaded, which is when the command - * line is parsed; that is why it takes the tick length explicitly. - * - * @param seconds Duration in simulated seconds. - * @param us_per_tick Simulated microseconds per firmware tick. - * @return Number of whole ticks that fit in the duration. - */ - static uint64_t total_ticks(double seconds, uint32_t us_per_tick); - /** * @brief Get how many ticks a duration spans. * @@ -97,14 +80,6 @@ class Clock { */ uint64_t tick_at(double seconds) const; - /** - * @brief Get how many ticks Stopwatch::elapsed_time_ms needs to report a millisecond count. - * - * @param target_ms Millisecond value the firmware must read. - * @return Tick count reproducing exactly that reading. - */ - uint64_t ticks_for_elapsed_ms(uint32_t target_ms) const; - private: /** * @brief Simulated microseconds elapsed since the start of the run. diff --git a/include/micras/sim/core/firmware_thread.hpp b/engine/include/micras/sim/core/firmware_thread.hpp similarity index 88% rename from include/micras/sim/core/firmware_thread.hpp rename to engine/include/micras/sim/core/firmware_thread.hpp index fc64821..740d35e 100644 --- a/include/micras/sim/core/firmware_thread.hpp +++ b/engine/include/micras/sim/core/firmware_thread.hpp @@ -31,10 +31,10 @@ class RunFinished : public std::exception { * @brief Strict handoff between the simulation and the firmware program. * * @note The two threads never run at the same time: exactly one of them is - * awake at any moment, so the run stays as deterministic as it was when - * the firmware was called inline. The firmware gives control back from - * inside its own loop, through yield_tick(), which is reached from the - * Stopwatch the firmware waits on. + * awake at any moment, so the handoff adds no nondeterminism. The firmware + * gives control back from inside its own loop, through yield_tick(), + * which the host timer's handover calls when a read crosses the end of a + * step. * * @note The simulation side is single threaded: run_until_yield() and finish() * must be called from the one thread that drives the run. @@ -42,9 +42,12 @@ class RunFinished : public std::exception { class FirmwareThread { public: /** - * @brief The firmware program to run, typically the Micras loop. + * @brief The firmware program to run: the target's main, given the thread it runs on. + * + * @note A program that drives the handover itself calls yield_tick() on the + * thread it is given. */ - using Program = std::function; + using Program = std::function; /** * @brief Take the program to run. @@ -130,8 +133,8 @@ class FirmwareThread { /** * @brief Body of the firmware thread. * - * @note Not called main: the hardware tests are compiled with that - * identifier redefined, so nothing reachable from them may use it. + * @note Not called main: the firmware is compiled with -Dmain=..., which + * would rename a member called main too. */ void thread_body(); diff --git a/engine/include/micras/sim/core/mujoco_world.hpp b/engine/include/micras/sim/core/mujoco_world.hpp new file mode 100644 index 0000000..a184290 --- /dev/null +++ b/engine/include/micras/sim/core/mujoco_world.hpp @@ -0,0 +1,265 @@ +/** + * @file + * + * @brief Owner of the MuJoCo model and data. + */ + +#ifndef MICRAS_SIM_CORE_MUJOCO_WORLD_HPP +#define MICRAS_SIM_CORE_MUJOCO_WORLD_HPP + +#include +#include +#include +#include + +#include + +namespace micras::sim { +/** + * @brief Loads a model, owns its data and resolves model objects by name. + * + * @note Every accessor throws a clear error when the model is not loaded yet, + * so a device built too early fails at construction instead of + * dereferencing a null pointer in the middle of a run. + */ +class MujocoWorld { +public: + /** + * @brief Geom group the range sensors do not see: the robot's own geoms, and what is only paint + * on the arena. + */ + static constexpr int unseen_group{3}; + + MujocoWorld() = default; + + MujocoWorld(const MujocoWorld&) = delete; + MujocoWorld(MujocoWorld&&) = delete; + MujocoWorld& operator=(const MujocoWorld&) = delete; + MujocoWorld& operator=(MujocoWorld&&) = delete; + + /** + * @brief Free the MuJoCo model and data. + */ + ~MujocoWorld(); + + /** + * @brief Load a model and allocate its data, replacing any previous one. + * + * @note Automatic reset is disabled on the loaded model so a diverging step + * is reported instead of silently restarting the run. + * + * @param model_path Path to the model XML file. + */ + void load(const std::filesystem::path& model_path); + + /** + * @brief Where a robot is placed in its arena. + */ + struct Placement { + double x{0.0}; + double y{0.0}; + double yaw{0.0}; + }; + + /** + * @brief Compose a robot and an arena into one model and allocate its data. + * + * @note The robot's model is the root, so its simulation options hold; the + * arena's body is attached to the world with the prefix "_", and + * the robot's root body is moved to the placement. The composed MJCF is + * kept, so a run can save exactly what it simulated. + * + * @param robot_xml MJCF of the robot, standing at the origin. + * @param robot_body Name of the robot's root body. + * @param arena_xml MJCF of the arena, holding its geoms in one body. + * @param arena_body Name of that body. + * @param placement Where the robot starts. + */ + void build( + const std::string& robot_xml, std::string_view robot_body, const std::string& arena_xml, + std::string_view arena_body, const Placement& placement + ); + + /** + * @brief Get the MJCF of what was built. + * + * @return The composed model, empty after load(). + */ + const std::string& composed_xml() const { return this->composed; } + + /** + * @brief Reset the simulation to its initial state and make sensordata valid. + */ + void reset(); + + /** + * @brief Advance the physics by a number of steps. + * + * @note Before each step, every sliding friction added to the world pushes + * on its geom's body through mjData::qfrc_applied, which the world + * owns: nothing else may write it. + * + * @param steps Number of mj_step calls. + */ + void step(int steps); + + /** + * @brief Give a frictionless geom a sliding friction, applied as a force + * instead of through MuJoCo's contact. + * + * @note MuJoCo's convex contact separates two surfaces in proportion to how + * fast their friction is slipping, so a geom that slides all the time, + * as a skid does, would lift what it carries off the floor. This + * friction pushes at each contact of the geom against the sliding + * velocity there, with the coefficient times the contact's normal + * force of the last step. Below sticking_speed it falls linearly to + * zero, which keeps it stable and stands in for static friction. + * Building or loading another model forgets every friction added. + * + * @param geom Name of the geom, which should have condim 1. + * @param coefficient Coefficient of sliding friction. + */ + void add_sliding_friction(const std::string& geom, double coefficient); + + /** + * @brief Sliding speed under which a sliding friction falls linearly to zero, in m/s. + */ + static constexpr double sticking_speed{0.005}; + + /** + * @brief Resolve a named MuJoCo object, throwing if it does not exist. + * + * @param type MuJoCo object type. + * @param name Name of the object. + * @return Id of the object. + */ + int require_id(mjtObj type, const std::string& name) const; + + /** + * @brief Resolve the sensordata address of a named sensor. + * + * @param name Name of the sensor. + * @return Index into mjData::sensordata. + */ + int sensor_address(const std::string& name) const; + + /** + * @brief Resolve where a named joint's first velocity dof sits in qvel. + * + * @param name Name of the joint. + * @return Index into mjData::qvel. + */ + int joint_dof(const std::string& name) const; + + /** + * @brief Resolve where a named joint's first position coordinate sits in qpos. + * + * @param name Name of the joint. + * @return Index into mjData::qpos. + */ + int joint_qpos(const std::string& name) const; + + /** + * @brief Read one sensordata entry. + * + * @param address Index into mjData::sensordata. + * @return Value of the sensor channel. + */ + double sensor_value(int address) const; + + /** + * @brief Write one actuator control value. + * + * @param actuator_id Id of the actuator. + * @param value Control value. + */ + void set_control(int actuator_id, double value); + + /** + * @brief Get the timestep the model integrates with. + * + * @return Timestep in seconds. + */ + double timestep() const; + + /** + * @brief Get the simulated time of the loaded data. + * + * @return Simulated seconds since the last reset. + */ + double time() const; + + /** + * @brief Get the loaded model. + * + * @note Exposed for the devices, the recorders and the renderer, which + * read the model's arrays directly. + * + * @return Pointer to the model. + */ + ///@{ + const mjModel* model() const; + mjModel* model(); + ///@} + + /** + * @brief Get the simulation data. + * + * @note Exposed for the devices, the recorders and the renderer, which + * read and write the state's arrays directly. + * + * @return Pointer to the data. + */ + ///@{ + const mjData* data() const; + mjData* data(); + ///@} + +private: + /** + * @brief A geom whose friction the world applies as a force. + */ + struct SlidingFriction { + int geom; + int body; + double coefficient; + }; + + /** + * @brief Write the force of every sliding friction into mjData::qfrc_applied. + */ + void apply_sliding_friction(); + + /** + * @brief Throw if no model is loaded. + */ + void require_loaded() const; + + /** + * @brief Release the model and data, if any. + */ + void unload(); + + /** + * @brief Loaded MuJoCo model. + */ + mjModel* model_handle{nullptr}; + + /** + * @brief Simulation data of the loaded model. + */ + mjData* data_handle{nullptr}; + + /** + * @brief MJCF of the last composed model. + */ + std::string composed; + + /** + * @brief Geoms whose friction the world applies. + */ + std::vector sliding_frictions; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_MUJOCO_WORLD_HPP diff --git a/engine/include/micras/sim/core/noise.hpp b/engine/include/micras/sim/core/noise.hpp new file mode 100644 index 0000000..ca6c94e --- /dev/null +++ b/engine/include/micras/sim/core/noise.hpp @@ -0,0 +1,72 @@ +/** + * @file + * + * @brief Seeded, reproducible noise, one independent stream per device. + */ + +#ifndef MICRAS_SIM_CORE_NOISE_HPP +#define MICRAS_SIM_CORE_NOISE_HPP + +#include +#include +#include + +namespace micras::sim { +/** + * @brief The run's noise settings: a seed, and whether noise is on at all. + */ +struct NoiseConfig { + /** + * @brief Seed every stream derives from; recorded in meta.json. + */ + uint64_t seed{1}; + + /** + * @brief Whether the world is ideal: no noise, no bias, no scale error. + */ + bool ideal{false}; +}; + +/** + * @brief A stream of Gaussian samples for one device. + * + * @note The generator is std::mt19937_64, whose sequence the standard fixes, and + * the Gaussian comes from a Box-Muller transform written here, because the + * standard distributions are free to differ between library versions. Each + * stream's seed mixes the run's seed with the stream's name, so adding a + * device never changes the noise another device draws. + */ +class Noise { +public: + /** + * @brief Open a stream. + * + * @param config The run's noise settings. + * @param stream Name of the stream, usually the device's. + */ + Noise(const NoiseConfig& config, std::string_view stream); + + /** + * @brief Draw from a centered Gaussian. + * + * @param sigma Standard deviation. + * @return A sample, or 0 when the world is ideal. + */ + double gaussian(double sigma); + +private: + /** + * @brief Draw from the unit interval, excluding zero. + * + * @return A sample in (0, 1]. + */ + double unit(); + + std::mt19937_64 engine; + bool on; + bool has_spare{false}; + double spare{0.0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_NOISE_HPP diff --git a/engine/include/micras/sim/core/run_context.hpp b/engine/include/micras/sim/core/run_context.hpp new file mode 100644 index 0000000..20d2a7a --- /dev/null +++ b/engine/include/micras/sim/core/run_context.hpp @@ -0,0 +1,55 @@ +/** + * @file + * + * @brief The world, the clock and the byte channel one run advances. + */ + +#ifndef MICRAS_SIM_CORE_RUN_CONTEXT_HPP +#define MICRAS_SIM_CORE_RUN_CONTEXT_HPP + +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/noise.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/devices/device.hpp" + +namespace micras::sim { +/** + * @brief Plain aggregate of what a run advances, with no logic of its own. + * + * @note Nothing here is a singleton. A robot target that has to reach the run + * from code it cannot construct, such as firmware globals initialized + * before main, owns one and hands it to the application. + */ +struct RunContext { + /** + * @brief MuJoCo model and data. + */ + MujocoWorld world; + + /** + * @brief Simulated time and tick arithmetic. + */ + Clock clock; + + /** + * @brief Byte channel to and from the firmware. + */ + SerialBus serial; + + /** + * @brief Seed and switch of every device's noise. + */ + NoiseConfig noise; + + /** + * @brief The simulated board, in the order the run drives it. + */ + std::vector> devices; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_RUN_CONTEXT_HPP diff --git a/engine/include/micras/sim/core/serial_bus.hpp b/engine/include/micras/sim/core/serial_bus.hpp new file mode 100644 index 0000000..3f33e63 --- /dev/null +++ b/engine/include/micras/sim/core/serial_bus.hpp @@ -0,0 +1,122 @@ +/** + * @file + * + * @brief The byte stream between a firmware's radio link and everything outside. + */ + +#ifndef MICRAS_SIM_CORE_SERIAL_BUS_HPP +#define MICRAS_SIM_CORE_SERIAL_BUS_HPP + +#include +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief Receives every byte the firmware sends. + */ +class ISerialListener { +public: + ISerialListener() = default; + + ISerialListener(const ISerialListener&) = delete; + ISerialListener(ISerialListener&&) = delete; + ISerialListener& operator=(const ISerialListener&) = delete; + ISerialListener& operator=(ISerialListener&&) = delete; + + virtual ~ISerialListener() = default; + + /** + * @brief Handle bytes the firmware just sent. + * + * @param bytes Bytes leaving the firmware, in stream order. + */ + virtual void on_firmware_bytes(std::span bytes) = 0; +}; + +/** + * @brief Byte stream between the firmware and everything listening to it. + * + * @note Bytes only, no framing: the protocol is the firmware's business. The + * link device moves bytes between this bus and the firmware's UART at the + * line's baud rate, so how fast a producer queues them never changes when + * they arrive. Output is fanned out to every listener as it leaves, so no + * listener can consume bytes another one needed. + */ +class SerialBus { +public: + /** + * @brief Largest number of bytes allowed to wait for the firmware. + */ + static constexpr std::size_t max_pending_bytes{65536}; + + /** + * @brief Start delivering firmware output to a listener. + * + * @note Listeners must not be added or removed while bytes are being + * delivered. Adding the same listener twice is ignored. + * + * @param listener Listener to add; it must outlive the bus or remove itself. + */ + void add_listener(ISerialListener& listener); + + /** + * @brief Stop delivering firmware output to a listener. + * + * @param listener Listener to remove; unknown listeners are ignored. + */ + void remove_listener(const ISerialListener& listener); + + /** + * @brief Deliver bytes the firmware just sent to every listener. + * + * @param bytes Bytes leaving the firmware. + */ + void send_from_firmware(std::span bytes); + + /** + * @brief Queue bytes for the firmware to receive. + * + * @note Bytes past the bound are dropped and counted: a producer that + * outruns the line would otherwise add latency without limit. + * + * @param bytes Bytes to send. + */ + void queue_for_firmware(std::span bytes); + + /** + * @brief Take the next bytes for the firmware. + * + * @param count Most bytes to take. + * @return The oldest queued bytes, at most count of them. + */ + std::vector take_for_firmware(std::size_t count); + + /** + * @brief Get how many bytes were dropped because the queue was full. + * + * @return Number of dropped bytes, always 0 on a healthy run. + */ + uint64_t dropped_bytes() const { return this->dropped; } + +private: + /** + * @brief Listeners receiving firmware output, in registration order. + */ + std::vector listeners; + + /** + * @brief Bytes waiting for the firmware. + */ + std::deque to_firmware; + + /** + * @brief Bytes dropped because the queue was full. + */ + uint64_t dropped{0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_SERIAL_BUS_HPP diff --git a/include/micras/sim/core/simulation.hpp b/engine/include/micras/sim/core/simulation.hpp similarity index 92% rename from include/micras/sim/core/simulation.hpp rename to engine/include/micras/sim/core/simulation.hpp index 2d96094..d7ccee4 100644 --- a/include/micras/sim/core/simulation.hpp +++ b/engine/include/micras/sim/core/simulation.hpp @@ -11,7 +11,7 @@ #include #include "micras/sim/core/firmware_thread.hpp" -#include "micras/sim/core/simulation_context.hpp" +#include "micras/sim/core/run_context.hpp" namespace micras::sim { /** @@ -91,7 +91,7 @@ class Simulation { * @param context Simulation the run advances. * @param firmware Program to run in lockstep with it. */ - Simulation(SimulationContext& context, FirmwareThread& firmware); + Simulation(RunContext& context, FirmwareThread& firmware); /** * @brief Add a listener, called in registration order. @@ -126,7 +126,7 @@ class Simulation { * * @return The context. */ - const SimulationContext& context() const { return this->simulation; } + const RunContext& context() const { return this->simulation; } private: /** @@ -142,6 +142,9 @@ class Simulation { /** * @brief Advance the physics and the clock by one tick. + * + * @note The devices act on the world first, from what the firmware wrote + * during the tick, and sample it last, into what it will read. */ void step(); @@ -151,7 +154,7 @@ class Simulation { * @note Bound for the life of the run; the context and the program both * outlive every simulation that drives them. */ - SimulationContext& simulation; // NOLINT(*-avoid-const-or-ref-data-members) + RunContext& simulation; // NOLINT(*-avoid-const-or-ref-data-members) /** * @brief Program running in lockstep. diff --git a/engine/include/micras/sim/core/span_at.hpp b/engine/include/micras/sim/core/span_at.hpp new file mode 100644 index 0000000..2155297 --- /dev/null +++ b/engine/include/micras/sim/core/span_at.hpp @@ -0,0 +1,38 @@ +/** + * @file + * + * @brief Checked element access for std::span, which has no at() before C++26. + */ + +#ifndef MICRAS_SIM_CORE_SPAN_AT_HPP +#define MICRAS_SIM_CORE_SPAN_AT_HPP + +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief Get an element of a span, checking the index as std::vector::at does. + * + * @note MuJoCo's arrays reach the simulator as spans over its buffers; an index past the end is a bug + * in the caller, reported as an exception instead of a read out of bounds. + * + * @tparam T Element type. + * @tparam Extent Extent of the span. + * @param values Span to read. + * @param index Index of the element. + * @return Reference to the element. + */ +template +constexpr T& at(std::span values, std::size_t index) { + if (index >= values.size()) { + throw std::out_of_range("span index out of range"); + } + + return *std::next(values.begin(), static_cast(index)); +} +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_SPAN_AT_HPP diff --git a/engine/include/micras/sim/core/text_file.hpp b/engine/include/micras/sim/core/text_file.hpp new file mode 100644 index 0000000..5e62098 --- /dev/null +++ b/engine/include/micras/sim/core/text_file.hpp @@ -0,0 +1,38 @@ +/** + * @file + * + * @brief Reading a whole text file, for the loaders of the robot, the scenario and the maze. + */ + +#ifndef MICRAS_SIM_CORE_TEXT_FILE_HPP +#define MICRAS_SIM_CORE_TEXT_FILE_HPP + +#include +#include +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief Read a whole text file. + * + * @param path Path of the file. + * @param what What the file holds, for the error message. + * @return The contents. + */ +inline std::string read_text_file(const std::filesystem::path& path, std::string_view what) { + std::ifstream file(path); + + if (not file.is_open()) { + throw std::runtime_error("cannot read the " + std::string{what} + " " + path.string()); + } + + std::ostringstream text; + text << file.rdbuf(); + return text.str(); +} +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_TEXT_FILE_HPP diff --git a/engine/include/micras/sim/core/variable_source.hpp b/engine/include/micras/sim/core/variable_source.hpp new file mode 100644 index 0000000..e4c91ae --- /dev/null +++ b/engine/include/micras/sim/core/variable_source.hpp @@ -0,0 +1,50 @@ +/** + * @file + * + * @brief Named firmware variables, as whatever decodes them reports them. + */ + +#ifndef MICRAS_SIM_CORE_VARIABLE_SOURCE_HPP +#define MICRAS_SIM_CORE_VARIABLE_SOURCE_HPP + +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief Names of the values of the variables that hold a state, by variable, starting at zero. + */ +using StateNames = std::map, std::less<>>; + +/** + * @brief Looks up the current value of a firmware variable by name. + * + * @note What the panel plots, the video overlay prints, the event log watches + * and the scenario conditions wait for. The engine never knows how the + * values are obtained; a robot target implements this over its own + * telemetry. + */ +class VariableSource { +public: + VariableSource() = default; + + VariableSource(const VariableSource&) = delete; + VariableSource(VariableSource&&) = delete; + VariableSource& operator=(const VariableSource&) = delete; + VariableSource& operator=(VariableSource&&) = delete; + + virtual ~VariableSource() = default; + + /** + * @brief Get the current value of a variable. + * + * @param name Firmware name of the variable. + * @return Its value, or NaN when it is unknown or not reported yet. + */ + virtual double value_of(const std::string& name) const = 0; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_CORE_VARIABLE_SOURCE_HPP diff --git a/engine/include/micras/sim/devices/dc_motor.hpp b/engine/include/micras/sim/devices/dc_motor.hpp new file mode 100644 index 0000000..e4e4f26 --- /dev/null +++ b/engine/include/micras/sim/devices/dc_motor.hpp @@ -0,0 +1,112 @@ +/** + * @file + * + * @brief An H-bridge driving a brushed DC motor through a gearbox. + */ + +#ifndef MICRAS_SIM_DEVICES_DC_MOTOR_HPP +#define MICRAS_SIM_DEVICES_DC_MOTOR_HPP + +#include +#include +#include + +#include "micras/sim/devices/device.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief Turns the bridge's two PWM inputs and its enable into the winding voltage. + * + * @note The motor itself is the robot model's actuator, whose control is the + * winding voltage (see robot_mjcf()). The bridge applies the duty cycle + * difference times the supply: with slow decay both inputs low short the + * winding, which brakes. A disabled bridge leaves the winding open, so no + * current flows, which this device expresses by applying exactly the + * back-EMF. + */ +class DcMotor : public Device { +public: + /** + * @brief What the motor is and what drives it. + */ + struct Config { + /** + * @brief Prefix of the recorded columns. + */ + std::string name; + + /** + * @brief Actuator and wheel joint of the robot model. + */ + ///@{ + std::string actuator; + std::string joint; + ///@} + + /** + * @brief Motor, gearbox and supply. + */ + DriveDescription drive; + + /** + * @brief Duty cycles of the bridge's inputs, in percent. + */ + ///@{ + std::function forward_duty; + std::function backward_duty; + ///@} + + /** + * @brief Level of the bridge's enable input. + */ + std::function enabled; + }; + + /** + * @brief Find the actuator and the joint in the model. + * + * @param world Loaded world. + * @param config The motor. + */ + DcMotor(const MujocoWorld& world, Config config); + + /** + * @brief Apply the winding voltage. + * + * @param world World about to be advanced. + * @param clock Clock of the run. + */ + void actuate(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the recorded columns: the winding voltage and current. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the voltage and current applied this tick. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + + /** + * @brief Get the winding current of this tick. + * + * @return Current in amperes, positive driving forward. + */ + double current() const { return this->winding_current; } + +private: + Config config; + int actuator_id; + int velocity_address; + double voltage{0.0}; + double winding_current{0.0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_DC_MOTOR_HPP diff --git a/engine/include/micras/sim/devices/device.hpp b/engine/include/micras/sim/devices/device.hpp new file mode 100644 index 0000000..e867289 --- /dev/null +++ b/engine/include/micras/sim/devices/device.hpp @@ -0,0 +1,70 @@ +/** + * @file + * + * @brief A simulated piece of hardware between the firmware's ports and the physics. + */ + +#ifndef MICRAS_SIM_DEVICES_DEVICE_HPP +#define MICRAS_SIM_DEVICES_DEVICE_HPP + +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +/** + * @brief One device of the simulated board: a motor, a sensor, a link. + * + * @note A device reaches the firmware only through the callables its + * configuration holds, which a robot target's bindings point at the host + * backend's ports; the engine never sees a HAL type. Every tick the run + * calls actuate() on every device, advances the physics, then calls + * sample() on every device, in the order they were added. + */ +class Device { +public: + Device() = default; + + Device(const Device&) = delete; + Device(Device&&) = delete; + Device& operator=(const Device&) = delete; + Device& operator=(Device&&) = delete; + + virtual ~Device() = default; + + /** + * @brief Turn what the firmware wrote into what the physics feels. + * + * @param world World about to be advanced. + * @param clock Clock, still at the start of the tick. + */ + virtual void actuate(MujocoWorld& world, const Clock& clock); + + /** + * @brief Turn the physics into what the firmware will read. + * + * @param world World that just advanced. + * @param clock Clock, already at the end of the tick. + */ + virtual void sample(MujocoWorld& world, const Clock& clock); + + /** + * @brief Get the names of the columns this device records. + * + * @return Column names, possibly none. + */ + virtual std::vector columns() const; + + /** + * @brief Append this device's cells to the current row. + * + * @param row Row being built; one cell per column. + */ + virtual void append(std::vector& row) const; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_DEVICE_HPP diff --git a/engine/include/micras/sim/devices/device_columns.hpp b/engine/include/micras/sim/devices/device_columns.hpp new file mode 100644 index 0000000..f032b60 --- /dev/null +++ b/engine/include/micras/sim/devices/device_columns.hpp @@ -0,0 +1,62 @@ +/** + * @file + * + * @brief The columns every device of a run records, in the order they were added. + */ + +#ifndef MICRAS_SIM_DEVICES_DEVICE_COLUMNS_HPP +#define MICRAS_SIM_DEVICES_DEVICE_COLUMNS_HPP + +#include +#include +#include + +#include "micras/sim/devices/device.hpp" +#include "micras/sim/recording/column_source.hpp" + +namespace micras::sim { +/** + * @brief Records what crossed the boundary between the firmware and the world. + */ +class DeviceColumns : public ColumnSource { +public: + /** + * @brief Take the run's devices. + * + * @param devices The devices; they must outlive the recorder. + */ + explicit DeviceColumns(const std::vector>& devices) : devices{devices} { } + + /** + * @brief Get every device's column names. + * + * @return Column names. + */ + std::vector names() override { + std::vector names; + + for (const auto& device : this->devices) { + const std::vector columns = device->columns(); + names.insert(names.end(), columns.begin(), columns.end()); + } + + return names; + } + + /** + * @brief Append every device's cells. + * + * @param row Row being built. + */ + void append(std::vector& row) override { + for (const auto& device : this->devices) { + device->append(row); + } + } + +private: + const std::vector>& devices; // NOLINT(*-avoid-const-or-ref-data-members): the run's. +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_DEVICE_COLUMNS_HPP diff --git a/engine/include/micras/sim/devices/digital_input.hpp b/engine/include/micras/sim/devices/digital_input.hpp new file mode 100644 index 0000000..b68d05c --- /dev/null +++ b/engine/include/micras/sim/devices/digital_input.hpp @@ -0,0 +1,79 @@ +/** + * @file + * + * @brief A button or a switch, as the firmware's pin sees it. + */ + +#ifndef MICRAS_SIM_DEVICES_DIGITAL_INPUT_HPP +#define MICRAS_SIM_DEVICES_DIGITAL_INPUT_HPP + +#include +#include +#include + +#include "micras/sim/devices/device.hpp" + +namespace micras::sim { +/** + * @brief A two-position input a scenario or a human sets: a button, a switch. + */ +class DigitalInput : public Device { +public: + /** + * @brief The input and the pin it drives. + */ + struct Config { + std::string name; + bool active_low; + std::function drive; + }; + + /** + * @brief Take the input, released. + * + * @param config The input. + */ + explicit DigitalInput(Config config); + + /** + * @brief Press or release it. + * + * @param active True for pressed, or on. + */ + void set(bool active); + + /** + * @brief Check whether it is pressed, or on. + * + * @return The state. + */ + bool is_active() const { return this->active; } + + /** + * @brief Get the name. + * + * @return The name. + */ + const std::string& name() const { return this->config.name; } + + /** + * @brief Get the recorded column: the state. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the state. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + Config config; + bool active{false}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_DIGITAL_INPUT_HPP diff --git a/engine/include/micras/sim/devices/imu.hpp b/engine/include/micras/sim/devices/imu.hpp new file mode 100644 index 0000000..4d1cf5b --- /dev/null +++ b/engine/include/micras/sim/devices/imu.hpp @@ -0,0 +1,120 @@ +/** + * @file + * + * @brief A MEMS inertial measurement unit, with its datasheet imperfections. + */ + +#ifndef MICRAS_SIM_DEVICES_IMU_HPP +#define MICRAS_SIM_DEVICES_IMU_HPP + +#include +#include +#include +#include +#include + +#include "micras/sim/core/noise.hpp" +#include "micras/sim/devices/device.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief Produces the chip's raw gyroscope and accelerometer words. + * + * @note The truth comes from MuJoCo's gyro and accelerometer sensors on a site + * whose frame is the chip's, so the axes are the chip's. Each output + * sample is that truth low pass filtered at the configured bandwidth, + * the gyroscope's scaled by a turn-on scale error, offset by a turn-on + * bias, with white noise at the datasheet density over the filter's noise + * bandwidth, then quantized to the configured resolution and delivered + * one sample late, as the chip's pipeline does. Samples come at the output rate times a + * small seeded clock error, so the firmware sometimes finds no new sample + * and sometimes finds one skipped, as with the real chip's own oscillator. + * + * @note MuJoCo computes sensors at the start of a step, so the truth lags the + * end of the tick by one physics step. + */ +class Imu : public Device { +public: + /** + * @brief The chip and where its samples go. + */ + struct Config { + /** + * @brief Prefix of the recorded columns. + */ + std::string name; + + /** + * @brief Gyro and accelerometer sensors of the robot model. + */ + ///@{ + std::string gyro; + std::string accelerometer; + ///@} + + /** + * @brief Datasheet and configuration values. + */ + ImuDescription description; + + /** + * @brief Receives each sample: gyroscope x, y, z then accelerometer x, y, z, in raw words. + */ + std::function)> write; + }; + + /** + * @brief Find the sensors and draw the turn-on errors. + * + * @param world Loaded world. + * @param config The chip. + * @param noise The run's noise settings. + */ + Imu(const MujocoWorld& world, Config config, const NoiseConfig& noise); + + /** + * @brief Filter the truth and deliver the samples that fell in this tick. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the recorded columns: the six readings in SI units. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the last delivered sample. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + /** + * @brief Values per sample. + */ + static constexpr std::size_t channels{6}; + + Config config; + Noise noise; + int gyro_address; + int accelerometer_address; + std::array filtered{}; + std::array bias{}; + std::array scale{}; + std::array pending{}; + std::array delivered{}; + double rate_error{0.0}; + double phase{0.0}; + bool has_pending{false}; + bool started{false}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_IMU_HPP diff --git a/engine/include/micras/sim/devices/power.hpp b/engine/include/micras/sim/devices/power.hpp new file mode 100644 index 0000000..8546da5 --- /dev/null +++ b/engine/include/micras/sim/devices/power.hpp @@ -0,0 +1,185 @@ +/** + * @file + * + * @brief The battery, the suction fan and the motor current sensors. + */ + +#ifndef MICRAS_SIM_DEVICES_POWER_HPP +#define MICRAS_SIM_DEVICES_POWER_HPP + +#include +#include +#include +#include +#include + +#include "micras/sim/core/noise.hpp" +#include "micras/sim/devices/device.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief A battery measured by an ADC through a divider. + * + * @note The open circuit voltage of charged cells, which the loads do not pull + * down. + */ +class Battery : public Device { +public: + /** + * @brief The pack and its measurement. + */ + struct Config { + std::string name; + BatteryDescription description; + double divider; + double adc_reference; + double adc_max_counts; + double adc_noise_counts; + std::function write; + }; + + /** + * @brief Take the pack. + * + * @param config The pack. + * @param noise The run's noise settings. + */ + Battery(Config config, const NoiseConfig& noise); + + /** + * @brief Convert the terminal voltage. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the terminal voltage. + * + * @return Voltage in volts. + */ + double voltage() const { return this->terminal_voltage; } + + /** + * @brief Get the recorded column: the terminal voltage. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the terminal voltage. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + Config config; + Noise noise; + double terminal_voltage; +}; + +/** + * @brief A suction fan pressing the robot down. + * + * @note The downforce goes as the square of the fan speed, which goes as the duty + * cycle times the supply, and follows its target with the motor's time + * constant. It is applied through the robot model's fan actuator. + */ +class Fan : public Device { +public: + /** + * @brief The fan and what drives it. + */ + struct Config { + std::string name; + std::string actuator; + FanDescription description; + std::function duty; + std::function enabled; + std::function supply_voltage; + }; + + /** + * @brief Find the actuator. + * + * @param world Loaded world. + * @param config The fan. + */ + Fan(const MujocoWorld& world, Config config); + + /** + * @brief Apply the downforce. + * + * @param world World about to be advanced. + * @param clock Clock of the run. + */ + void actuate(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the recorded column: the downforce. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the downforce. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + Config config; + int actuator_id; + double downforce{0.0}; +}; + +/** + * @brief Current sense amplifiers read by an ADC. + */ +class CurrentSense : public Device { +public: + /** + * @brief The channels and their scaling. + * + * @note The amplifiers are bidirectional: a channel reads the zero voltage with no current, and + * moves from it by the volts per amp in the direction of the current. + */ + struct Config { + std::vector> currents; + double zero_voltage; + double volts_per_amp; + double adc_reference; + double adc_max_counts; + double adc_noise_counts; + std::function write; + }; + + /** + * @brief Take the channels. + * + * @param config The channels. + * @param noise The run's noise settings. + */ + CurrentSense(Config config, const NoiseConfig& noise); + + /** + * @brief Convert every channel. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + +private: + Config config; + Noise noise; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_POWER_HPP diff --git a/engine/include/micras/sim/devices/quadrature_encoder.hpp b/engine/include/micras/sim/devices/quadrature_encoder.hpp new file mode 100644 index 0000000..c933352 --- /dev/null +++ b/engine/include/micras/sim/devices/quadrature_encoder.hpp @@ -0,0 +1,89 @@ +/** + * @file + * + * @brief An incremental encoder counting a joint's rotation. + */ + +#ifndef MICRAS_SIM_DEVICES_QUADRATURE_ENCODER_HPP +#define MICRAS_SIM_DEVICES_QUADRATURE_ENCODER_HPP + +#include +#include +#include +#include + +#include "micras/sim/devices/device.hpp" + +namespace micras::sim { +/** + * @brief Counts edges of a quadrature output, as a timer in encoder mode does. + * + * @note The count is the joint angle quantized to the encoder's resolution; a + * magnet on the wheel axle turns one to one with the wheel, so there is + * no backlash between them. + */ +class QuadratureEncoder : public Device { +public: + /** + * @brief What the encoder counts and where the count goes. + */ + struct Config { + /** + * @brief Name of the recorded column. + */ + std::string name; + + /** + * @brief Joint of the robot model. + */ + std::string joint; + + /** + * @brief Edges per revolution. + */ + uint32_t counts_per_revolution; + + /** + * @brief Receives the count after every tick. + */ + std::function write; + }; + + /** + * @brief Find the joint in the model. + * + * @param world Loaded world. + * @param config The encoder. + */ + QuadratureEncoder(const MujocoWorld& world, Config config); + + /** + * @brief Count the joint's angle. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the recorded column: the count. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append the count. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + Config config; + int position_address; + int32_t count{0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_QUADRATURE_ENCODER_HPP diff --git a/engine/include/micras/sim/devices/serial_link.hpp b/engine/include/micras/sim/devices/serial_link.hpp new file mode 100644 index 0000000..36c4639 --- /dev/null +++ b/engine/include/micras/sim/devices/serial_link.hpp @@ -0,0 +1,73 @@ +/** + * @file + * + * @brief A serial line between the firmware's UART and the outside world. + */ + +#ifndef MICRAS_SIM_DEVICES_SERIAL_LINK_HPP +#define MICRAS_SIM_DEVICES_SERIAL_LINK_HPP + +#include +#include +#include +#include + +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/devices/device.hpp" + +namespace micras::sim { +/** + * @brief Moves bytes both ways at the line's baud rate. + * + * @note Ten bit times per byte, 8N1. Each direction has its own budget, carried + * over between ticks, so the throughput is exactly the line's whatever the + * tick. The bus is the world side: its listeners hear what the firmware + * sends, and what is queued on it reaches the firmware. + */ +class SerialLink : public Device { +public: + /** + * @brief The line and the firmware's side of it. + */ + struct Config { + /** + * @brief Bits per second. + */ + uint32_t baud_rate; + + /** + * @brief Takes the next byte the firmware is sending, if any. + */ + std::function()> take_sent; + + /** + * @brief Puts a byte where the firmware receives it. + */ + std::function receive; + }; + + /** + * @brief Connect the line to the bus. + * + * @param bus The world side. + * @param config The line. + */ + SerialLink(SerialBus& bus, Config config); + + /** + * @brief Move this tick's bytes. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + +private: + SerialBus& bus; // NOLINT(*-avoid-const-or-ref-data-members): the run's bus outlives its devices. + Config config; + double send_budget{0.0}; + double receive_budget{0.0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_SERIAL_LINK_HPP diff --git a/engine/include/micras/sim/devices/wall_sensors.hpp b/engine/include/micras/sim/devices/wall_sensors.hpp new file mode 100644 index 0000000..b9a839c --- /dev/null +++ b/engine/include/micras/sim/devices/wall_sensors.hpp @@ -0,0 +1,161 @@ +/** + * @file + * + * @brief Infrared emitter and receiver pairs ranging walls by reflected light. + */ + +#ifndef MICRAS_SIM_DEVICES_WALL_SENSORS_HPP +#define MICRAS_SIM_DEVICES_WALL_SENSORS_HPP + +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/noise.hpp" +#include "micras/sim/devices/device.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief The ADC conversions of a set of wall sensors, lit and dark. + * + * @note The emitters fire in groups: every scan period one group is lit and every + * receiver is converted, the lit group's own receivers reading their + * emitter and the others reading whatever the lit group throws their way, + * which is the cross-talk. There are two groups, so the other group's scan + * is each sensor's dark reading. The sequence the firmware sees is one scan + * per group, in group order, and completes after the second one. + * + * @note Light model: each emitter is a cone of rays whose intensity falls off as + * 2^-(angle / half angle)^2 around its axis, down to 2.5 half angles. Each + * ray that hits the arena reflects as a Lambertian spot of the surface's + * reflectance; the receiver collects it by the inverse square of its + * distance, the cosine of the spot's emission angle and the receiver's own + * angular response. The photocurrent is the responsivity times that + * irradiance, plus ambient light, and becomes a voltage across the load + * resistor, clipped at saturation. The emitter's contribution reaches only + * the settled fraction of its value by the time the conversion starts. + */ +class WallSensors : public Device { +public: + /** + * @brief The sensors and where their conversions go. + */ + struct Config { + /** + * @brief Prefix of the recorded columns. + */ + std::string name; + + /** + * @brief Optics, electronics and poses; the robot model has an + * "_emitter" and an "_receiver" site per sensor. + */ + WallSensorsDescription description; + + /** + * @brief Ticks between two scans. + */ + uint32_t scan_ticks; + + /** + * @brief Duty cycle of a sensor's emitter, in percent; an emitter only lights when it is above zero. + */ + std::function emitter_duty; + + /** + * @brief Receives one conversion: its index in the sequence and its counts. + */ + std::function write; + + /** + * @brief Called when the sequence completes. + */ + std::function finish_sequence; + + /** + * @brief Reflectance of an arena geom, by id. + * + * @note The rays pass through the geoms of MujocoWorld::unseen_group. + */ + std::function reflectance; + }; + + /** + * @brief Find the sites and lay out the ray cone. + * + * @note Throws unless the sensors fall in exactly two groups. + * + * @param world Loaded world. + * @param config The sensors. + * @param noise The run's noise settings. + */ + WallSensors(const MujocoWorld& world, Config config, const NoiseConfig& noise); + + /** + * @brief Convert every receiver with the next group lit, on the ticks a scan falls on. + * + * @param world World that just advanced. + * @param clock Clock of the run. + */ + void sample(MujocoWorld& world, const Clock& clock) override; + + /** + * @brief Get the recorded columns: each sensor's lit minus dark reading, as the firmware normalizes it. + * + * @return Column names. + */ + std::vector columns() const override; + + /** + * @brief Append each sensor's last lit minus dark reading. + * + * @param row Row being built. + */ + void append(std::vector& row) const override; + +private: + /** + * @brief One ray of an emitter's cone, in the emitter's frame. + */ + struct Ray { + std::array direction; + double flux; + }; + + /** + * @brief Farthest distance a ray looks for a surface, in meters. + */ + static constexpr double cutoff{0.5}; + + /** + * @brief Compute the irradiance one emitter puts on one receiver. + * + * @param world The world. + * @param emitter Index of the emitting sensor. + * @param receiver Index of the receiving sensor. + * @return Irradiance in W/m^2. + */ + double irradiance(MujocoWorld& world, std::size_t emitter, std::size_t receiver); + + Config config; + Noise noise; + std::vector emitter_sites; + std::vector receiver_sites; + std::vector rays; + std::array groups{}; + std::vector counts; + std::vector intensities; + std::array geom_groups{}; + std::vector directions; + std::vector distances; + std::vector normals; + std::vector geoms; + uint64_t scans{0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_DEVICES_WALL_SENSORS_HPP diff --git a/engine/include/micras/sim/recording/column_source.hpp b/engine/include/micras/sim/recording/column_source.hpp new file mode 100644 index 0000000..6a6f658 --- /dev/null +++ b/engine/include/micras/sim/recording/column_source.hpp @@ -0,0 +1,58 @@ +/** + * @file + * + * @brief Columns a robot target adds to every CSV row. + */ + +#ifndef MICRAS_SIM_RECORDING_COLUMN_SOURCE_HPP +#define MICRAS_SIM_RECORDING_COLUMN_SOURCE_HPP + +#include +#include + +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +/** + * @brief A block of CSV columns the recorder writes after the ground truth. + * + * @note names() is asked once, when the first row is written, and no row is + * written before every source is ready, so a source whose columns depend + * on something the firmware reports only once it has started, such as the + * variables its robot registers, can still name them. + */ +class ColumnSource { +public: + ColumnSource() = default; + + ColumnSource(const ColumnSource&) = delete; + ColumnSource(ColumnSource&&) = delete; + ColumnSource& operator=(const ColumnSource&) = delete; + ColumnSource& operator=(ColumnSource&&) = delete; + + virtual ~ColumnSource() = default; + + /** + * @brief Check whether the source can name its columns and fill them. + * + * @return True once it can; always true unless the source says otherwise. + */ + virtual bool ready() const { return true; } + + /** + * @brief Get the column names, in the order append() fills them. + * + * @return Column names. + */ + virtual std::vector names() = 0; + + /** + * @brief Append this source's cells for the current row. + * + * @param row Row being built; exactly one cell per name must be appended. + */ + virtual void append(std::vector& row) = 0; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_RECORDING_COLUMN_SOURCE_HPP diff --git a/engine/include/micras/sim/recording/csv_recorder.hpp b/engine/include/micras/sim/recording/csv_recorder.hpp new file mode 100644 index 0000000..544d098 --- /dev/null +++ b/engine/include/micras/sim/recording/csv_recorder.hpp @@ -0,0 +1,103 @@ +/** + * @file + * + * @brief One CSV row per firmware tick: ground truth followed by the robot's own columns. + */ + +#ifndef MICRAS_SIM_RECORDING_CSV_RECORDER_HPP +#define MICRAS_SIM_RECORDING_CSV_RECORDER_HPP + +#include +#include +#include +#include + +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/recording/column_source.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/recording/ground_truth.hpp" + +namespace micras::sim { +/** + * @brief Samples the simulation state once per firmware tick into a CSV file. + * + * @note A row is the ground truth, then every column source in the order it + * was added. The header is written with the first row, and the first row + * is the first one due once every source is ready, so a source may learn + * its column names while the run goes. Two sources naming the same column + * is an error, not a second column nobody can tell apart. + */ +class CsvRecorder : public IRunListener { +public: + /** + * @brief Open the CSV file and resolve every model id the ground truth needs. + * + * @param world Simulation world to sample. + * @param config Robot body, forward axis and ground truth columns. + * @param path Path of the CSV file to write. + * @param every Ticks between two rows. + */ + CsvRecorder( + const MujocoWorld& world, GroundTruthConfig config, const std::filesystem::path& path, uint32_t every = 1 + ); + + /** + * @brief Add a block of columns after the ones already added. + * + * @param source Source to add; it must outlive the recorder. + */ + void add_source(ColumnSource& source); + + /** + * @brief Write one CSV row for the current simulation state, unless a source is not ready yet. + * + * @param tick Index of the firmware tick that was just executed. + */ + void sample(uint64_t tick); + + /** + * @brief Write the row of the tick that just ran, when it falls on the decimation. + * + * @param simulation Run that just advanced. + */ + void on_after_tick(const Simulation& simulation) override; + +private: + /** + * @brief Ask every source for its names and write the header. + */ + void write_header(); + + /** + * @brief Physical state sampler. + */ + GroundTruth ground_truth; + + /** + * @brief Output file. + */ + CsvWriter writer; + + /** + * @brief Column sources, in the order their columns appear. + */ + std::vector sources; + + /** + * @brief How many columns each source named, checked on every row. + */ + std::vector widths; + + /** + * @brief Ticks between two rows. + */ + uint32_t every; + + /** + * @brief Whether the header was already written. + */ + bool header_written{false}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_RECORDING_CSV_RECORDER_HPP diff --git a/include/micras/sim/recording/csv_writer.hpp b/engine/include/micras/sim/recording/csv_writer.hpp similarity index 100% rename from include/micras/sim/recording/csv_writer.hpp rename to engine/include/micras/sim/recording/csv_writer.hpp diff --git a/engine/include/micras/sim/recording/event_log.hpp b/engine/include/micras/sim/recording/event_log.hpp new file mode 100644 index 0000000..bcfc595 --- /dev/null +++ b/engine/include/micras/sim/recording/event_log.hpp @@ -0,0 +1,114 @@ +/** + * @file + * + * @brief What a run notices at full rate, whatever the recording's decimation. + */ + +#ifndef MICRAS_SIM_RECORDING_EVENT_LOG_HPP +#define MICRAS_SIM_RECORDING_EVENT_LOG_HPP + +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/recording/run_metadata.hpp" + +namespace micras::sim { +/** + * @brief Logs collisions and state changes every tick, for meta.json. + * + * @note A collision is a watched geom starting to touch anything but an ignored + * geom, such as the floor, after having been clear of everything but the + * ignored geoms for at least min_separation: a chassis pressed against a wall makes and breaks + * contact every few steps, and that is one collision, not hundreds. A + * state change is a watched variable taking a + * new value, named from its table when it has one. The log keeps the first + * entries only, so a robot stuck against a wall cannot grow meta.json + * without bound; the counts keep counting. + */ +class EventLog : public IRunListener { +public: + /** + * @brief Most entries kept. + */ + static constexpr std::size_t max_events{500}; + + /** + * @brief Time a geom has to stay clear for its next contact to be a new collision, in seconds. + */ + static constexpr double min_separation{0.05}; + + /** + * @brief Watch geoms and states. + * + * @param geoms Ids of the geoms whose collisions are logged. + * @param ignored Ids of the geoms touching which is not a collision. + * @param states Variables whose changes are logged, with the names of their values. + * @param variables Where the variables are read, or null. + */ + EventLog(std::vector geoms, std::set ignored, StateNames states, const VariableSource* variables); + + /** + * @brief Look for new contacts and new states. + * + * @param simulation Run that just advanced. + */ + void on_after_tick(const Simulation& simulation) override; + + /** + * @brief Get the entries, in time order. + * + * @return The entries kept. + */ + const std::vector& entries() const { return this->logged; } + + /** + * @brief Get how many collisions started. + * + * @return Number of collisions, counted past the entries kept. + */ + std::size_t collisions() const { return this->collision_count; } + +private: + /** + * @brief Keep an entry, while there is room. + * + * @param event The entry. + */ + void add(RunEvent event); + + /** + * @brief Log the collisions that started on this tick. + * + * @param model The model, naming the geoms. + * @param contacts The tick's contacts. + * @param now Simulated time, in seconds. + */ + void log_collisions(const mjModel* model, std::span contacts, double now); + + /** + * @brief Log the watched variables that changed on this tick. + * + * @param now Simulated time, in seconds. + */ + void log_states(double now); + + std::vector geoms; + std::set ignored; + StateNames states; + const VariableSource* variables; + std::vector last_touch; + std::vector> last_values; + std::vector logged; + std::size_t collision_count{0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_RECORDING_EVENT_LOG_HPP diff --git a/engine/include/micras/sim/recording/ground_truth.hpp b/engine/include/micras/sim/recording/ground_truth.hpp new file mode 100644 index 0000000..643b6d9 --- /dev/null +++ b/engine/include/micras/sim/recording/ground_truth.hpp @@ -0,0 +1,210 @@ +/** + * @file + * + * @brief Physical state of the robot read straight from MuJoCo. + */ + +#ifndef MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP +#define MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP + +#include +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +/** + * @brief Quantity one ground truth column reads. + * + * @note Joint probes name a joint, actuator probes an actuator and contact + * probes a geom. The solver iterations read the whole world and take no + * object. + */ +enum class Probe : uint8_t { + JOINT_VELOCITY, + JOINT_POSITION, + ACTUATOR_FORCE, + CONTACT_COUNT, + CONTACT_NORMAL_FORCE, + CONTACT_SLIP, + CONTACT_PENETRATION, + SOLVER_ITERATIONS, +}; + +/** + * @brief One robot-specific ground truth column. + */ +struct GroundTruthColumn { + /** + * @brief Column name in the CSV header. + */ + std::string name; + + /** + * @brief What the column reads. + */ + Probe probe; + + /** + * @brief Name of the MuJoCo joint, actuator or geom, empty for world probes. + */ + std::string object; +}; + +/** + * @brief What a robot target tells the recorder about its model. + */ +struct GroundTruthConfig { + /** + * @brief Robot body, whose first joint must be a free joint. + */ + std::string body; + + /** + * @brief Columns written after the body pose and velocity, in this order. + */ + std::vector columns; +}; + +/** + * @brief Resolves the model ids once and samples the ground truth columns every tick. + * + * @note Every row starts with the same block: tick, sim_time, the + * body pose and velocity, and v_forward. The robot's own columns follow, + * in the order its config lists them. + * + * @note MuJoCo free joints split their six velocity dofs across two frames: + * the three linear dofs are expressed in the world frame, the three + * angular dofs in the body frame. The columns are named accordingly + * (vx_world, vy_world, vz_world, wz_body) and v_forward projects the + * world linear velocity onto the body's x axis, which is forward. + */ +class GroundTruth { +public: + /** + * @brief Resolve every model id the columns need. + * + * @param world Loaded simulation world. + * @param config Robot body and columns. + */ + GroundTruth(const MujocoWorld& world, GroundTruthConfig config); + + /** + * @brief Get the column names, in the order sample() fills them. + * + * @return Column names. + */ + std::vector columns() const; + + /** + * @brief Sample the current physical state. + * + * @param tick Index of the firmware tick that was just executed. + * @return One cell per column. + */ + std::vector sample(uint64_t tick); + + /** + * @brief Sum every MuJoCo warning counter raised so far. + * + * @param world Loaded simulation world. + * @return Total number of warnings. + */ + static int64_t warnings_total(const MujocoWorld& world); + +private: + /** + * @brief Per-geom contact statistics for a single step. + */ + struct ContactStats { + int count{0}; + double normal_force{0.0}; + double slip{0.0}; + + /** + * @brief Deepest penetration over the geom's contacts, NaN when it has none. + */ + double penetration{std::numeric_limits::quiet_NaN()}; + }; + + /** + * @brief A column with its model address resolved. + */ + struct ResolvedColumn { + /** + * @brief What the column reads. + */ + Probe probe; + + /** + * @brief Index into the mjData array the probe reads, or the geom id. + */ + int address; + }; + + /** + * @brief Collect the contact statistics of one geom. + * + * @param geom_id Id of the geom. + * @return Aggregated statistics. + */ + ContactStats collect_contacts(int geom_id) const; + + /** + * @brief Get the contact statistics of one geom for the current row. + * + * @note Collected once per geom and row, however many columns read them. + * + * @param geom_id Id of the geom. + * @return Aggregated statistics. + */ + const ContactStats& contacts_of(int geom_id); + + /** + * @brief Read one robot-specific column. + * + * @param column Resolved column. + * @return Its cell. + */ + CsvCell read(const ResolvedColumn& column); + + /** + * @brief Simulation world being sampled. + * + * @note Bound for the life of the recorder; it is fed by the caller that + * also owns the run. + */ + const MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) + + /** + * @brief Robot body and columns. + */ + GroundTruthConfig config; + + /** + * @brief Id of the robot body. + */ + int body_id; + + /** + * @brief First velocity dof of the robot's free joint. + */ + int free_joint_qvel; + + /** + * @brief The robot-specific columns, resolved. + */ + std::vector resolved; + + /** + * @brief Contact statistics of the current row, per geom id. + */ + std::unordered_map row_contacts; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP diff --git a/include/micras/sim/recording/run_metadata.hpp b/engine/include/micras/sim/recording/run_metadata.hpp similarity index 51% rename from include/micras/sim/recording/run_metadata.hpp rename to engine/include/micras/sim/recording/run_metadata.hpp index 4cd99ef..2cab5bd 100644 --- a/include/micras/sim/recording/run_metadata.hpp +++ b/engine/include/micras/sim/recording/run_metadata.hpp @@ -11,8 +11,44 @@ #include #include #include +#include namespace micras::sim { +/** + * @brief A counter a robot target adds to meta.json. + */ +struct MetadataField { + /** + * @brief Key in meta.json. + */ + std::string name; + + /** + * @brief Value, written as a JSON integer. + */ + int64_t value{0}; +}; + +/** + * @brief Something the run noticed at full rate, whatever the recording's decimation. + */ +struct RunEvent { + /** + * @brief Simulated time, in seconds. + */ + double time{0.0}; + + /** + * @brief What happened, for example "collision" or "state". + */ + std::string kind; + + /** + * @brief With what, for example the geom or the state's name. + */ + std::string detail; +}; + /** * @brief Everything needed to reproduce and compare a run. * @@ -20,24 +56,66 @@ namespace micras::sim { * binary with the same arguments must produce byte-identical files. */ struct RunMetadata { + /** + * @brief Name of the robot target that ran, which the analysis plugins key on. + */ + std::string target; + + /** + * @brief Folder of the robot target, where the analysis tools find its plugin. + */ + std::string target_dir; + std::string firmware_sha; - std::string model_path; - std::string model_sha256; + std::string robot_path; + std::string robot_sha256; + std::string scenario_path; std::string maze_path; + + /** + * @brief Digest of the composed model, robot and arena, as saved in model.xml. + */ + std::string model_sha256; + std::string mujoco_version; std::string compiler; std::string build_type; std::string args; + uint64_t seed{0}; + bool ideal{false}; uint32_t loop_time_us{0}; double timestep{0.0}; int steps_per_tick{0}; + uint32_t record_every{1}; uint64_t requested_ticks{0}; uint64_t ticks{0}; double sim_time{0.0}; - double final_z{0.0}; - std::size_t pool_columns{0}; - uint32_t telemetry_resyncs{0}; - int64_t warnings_total{0}; + + /** + * @brief When the scenario's stop condition held, in seconds; negative when it never did. + */ + double stopped_at{-1.0}; + + double final_z{0.0}; + + /** + * @brief The robot target's own counters, written in this order after final_z. + */ + std::vector target_fields; + + int64_t warnings_total{0}; + + /** + * @brief Bytes for the firmware dropped because the link's queue was full. + */ + uint64_t serial_dropped_bytes{0}; + + /** + * @brief Frames dropped because a connected monitor did not keep up. + * + * @note Not behavior: frames leave after the tick and never reach the firmware. + */ + uint64_t bridge_dropped_frames{0}; /** * @brief Whether a human changed anything the physics could see. @@ -47,6 +125,11 @@ struct RunMetadata { */ bool interactive{false}; + /** + * @brief What the run noticed at full rate, in time order. + */ + std::vector events; + /** * @brief Serialize to the pretty-printed JSON layout of meta.json. * @@ -65,6 +148,14 @@ struct RunMetadata { */ void write(const std::filesystem::path& path) const; + /** + * @brief Compute the SHA-256 digest of a text. + * + * @param text The text. + * @return Lowercase hexadecimal digest. + */ + static std::string sha256_of_text(const std::string& text); + /** * @brief Compute the SHA-256 digest of a file. * diff --git a/engine/include/micras/sim/robot/robot_description.hpp b/engine/include/micras/sim/robot/robot_description.hpp new file mode 100644 index 0000000..0ac750c --- /dev/null +++ b/engine/include/micras/sim/robot/robot_description.hpp @@ -0,0 +1,258 @@ +/** + * @file + * + * @brief The physical description of a robot, read from its robot.toml. + */ + +#ifndef MICRAS_SIM_ROBOT_ROBOT_DESCRIPTION_HPP +#define MICRAS_SIM_ROBOT_ROBOT_DESCRIPTION_HPP + +#include +#include +#include +#include +#include + +namespace micras::sim { +/** + * @brief A point or a direction in the robot frame, in meters. + */ +using Vector3 = std::array; + +/** + * @brief A box in the robot frame, given by two opposite corners. + */ +struct BoxPart { + std::string name; + Vector3 min{}; + Vector3 max{}; +}; + +/** + * @brief A cylinder in the robot frame, its axis along robot z. + */ +struct CylinderPart { + std::string name; + Vector3 base{}; + double radius{}; + double height{}; +}; + +/** + * @brief A sphere under the chassis that carries it on the floor. + * + * @note It stands in for what the chassis rests on, a glide or the edge of its + * board, since a mesh edge on a plane is a poor contact in MuJoCo. Its + * contact has no friction (condim 1), so it only ever pushes up and + * never lifts itself as a slipping contact does; its friction, the + * coefficient of whatever slides on the floor there, is applied by the + * world as a force (MujocoWorld::add_sliding_friction). + */ +struct SkidPart { + std::string name; + Vector3 center{}; + double radius{}; + double friction{}; +}; + +/** + * @brief The rigid chassis: mass properties, outline and the parts that stick out of it. + */ +struct ChassisDescription { + double mass{}; + Vector3 center_of_mass{}; + Vector3 inertia{}; + std::vector> outline; + double board_bottom{}; + double board_top{}; + double friction{}; + std::vector boxes; + std::vector cylinders; + std::vector skids; +}; + +/** + * @brief Both drive wheels, mirrored about the robot's x axis. + */ +struct WheelsDescription { + double radius{}; + double width{}; + double track{}; + double mass{}; + double spin_inertia{}; + double friction{}; + double torsional_friction{}; + double contact_time_constant{}; + double contact_damping_ratio{}; +}; + +/** + * @brief One DC motor per wheel, its H-bridge and its gearbox. + */ +struct DriveDescription { + double winding_resistance{}; + double bridge_resistance{}; + double torque_constant{}; + double speed_constant{}; + double no_load_current{}; + double rotor_inertia{}; + double gear_ratio{}; + double gear_efficiency{}; + double supply_voltage{}; + + /** + * @brief Resistance a motor current flows through: winding and bridge. + * + * @return Resistance in ohms. + */ + double resistance() const { return this->winding_resistance + this->bridge_resistance; } +}; + +/** + * @brief The quadrature encoders on the wheel axles. + */ +struct EncoderDescription { + uint32_t counts_per_revolution{}; +}; + +/** + * @brief The inertial measurement unit: mounting, axes and datasheet noise. + */ +struct ImuDescription { + Vector3 position{}; + + /** + * @brief The chip's x and y axes, expressed in the robot frame; z is their cross product. + */ + std::array axes{}; + + double gyro_noise_density{}; + double gyro_bias{}; + double gyro_scale_error{}; + double gyro_resolution{}; + double accel_noise_density{}; + double accel_bias{}; + double accel_resolution{}; + double bandwidth{}; + double output_rate{}; + double clock_error{}; +}; + +/** + * @brief The suction fan: where it pulls and how hard. + */ +struct FanDescription { + Vector3 position{}; + double max_downforce{}; + double nominal_voltage{}; + double time_constant{}; +}; + +/** + * @brief One wall sensor: an emitter and a receiver side by side. + */ +struct WallSensorDescription { + std::string name; + Vector3 emitter{}; + Vector3 receiver{}; + double yaw{}; + int group{}; + + /** + * @brief Sensitivity of this pair relative to the nominal optics: part spread and alignment. + */ + double gain{}; +}; + +/** + * @brief The wall sensors' optics and electronics, shared by every sensor. + */ +struct WallSensorsDescription { + double emitter_half_angle{}; + double emitter_intensity{}; + double receiver_half_angle{}; + double receiver_responsivity{}; + double load_resistance{}; + double saturation_voltage{}; + double adc_reference{}; + double adc_max_counts{}; + double adc_noise_counts{}; + double settle_fraction{}; + double ambient_irradiance{}; + int rays{}; + + std::vector sensors; +}; + +/** + * @brief The battery the fan and the logic run from. + */ +struct BatteryDescription { + int cells{}; + double cell_voltage{}; +}; + +/** + * @brief The radio link the firmware talks through. + */ +struct LinkDescription { + uint32_t baud_rate{}; +}; + +/** + * @brief How the simulator integrates this robot. + */ +struct IntegrationDescription { + double timestep{}; +}; + +/** + * @brief Everything physical about one robot. + * + * @note Frames follow the firmware's robot model: x forward, y left, z up, the + * origin on the floor under the midpoint of the wheel axle. Every length is + * in meters, every angle in radians. Each value in the file is a number, an + * array, or a table { value = ..., source = "..." } naming where it came + * from: a datasheet, the CAD, the firmware, the owner, or an estimate. + */ +struct RobotDescription { + /** + * @brief Version of the schema this file follows. + */ + static constexpr int schema_version{1}; + + std::string name; + IntegrationDescription integration; + ChassisDescription chassis; + WheelsDescription wheels; + DriveDescription drive; + EncoderDescription encoders; + ImuDescription imu; + FanDescription fan; + WallSensorsDescription wall_sensors; + BatteryDescription battery; + LinkDescription link; + + /** + * @brief Read and check a description. + * + * @note Throws on a missing key, an unknown key, a wrong type or a schema + * version this build does not know, naming the key. + * + * @param path Path of the robot.toml. + * @return The description. + */ + static RobotDescription load(const std::filesystem::path& path); + + /** + * @brief Read and check a description from text. + * + * @param text Contents of a robot.toml. + * @param origin Name of the text, for messages. + * @return The description. + */ + static RobotDescription parse(std::string_view text, const std::string& origin = "robot.toml"); +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_ROBOT_ROBOT_DESCRIPTION_HPP diff --git a/engine/include/micras/sim/robot/robot_model.hpp b/engine/include/micras/sim/robot/robot_model.hpp new file mode 100644 index 0000000..9214e6c --- /dev/null +++ b/engine/include/micras/sim/robot/robot_model.hpp @@ -0,0 +1,68 @@ +/** + * @file + * + * @brief The MuJoCo model of a robot, generated from its description. + */ + +#ifndef MICRAS_SIM_ROBOT_ROBOT_MODEL_HPP +#define MICRAS_SIM_ROBOT_ROBOT_MODEL_HPP + +#include + +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief Names the generated model gives to what devices and recorders look up. + * + * @note One place for every name, so a device and the generator cannot disagree. + */ +struct RobotModelNames { + /** + * @brief The robot's body and free joint, the robot's own name. + */ + std::string body; + + ///@{ + std::string left_wheel{"left_wheel"}; + std::string right_wheel{"right_wheel"}; + std::string left_motor{"motor_left"}; + std::string right_motor{"motor_right"}; + std::string board{"board"}; + std::string fan{"fan"}; + std::string fan_reference{"fan_reference"}; + std::string imu{"imu"}; + std::string gyro{"imu_gyro"}; + std::string accelerometer{"imu_accelerometer"}; + + ///@} + + /** + * @brief Get the names a description's model uses. + * + * @param robot The description. + * @return The names. + */ + static RobotModelNames of(const RobotDescription& robot) { return {.body = robot.name}; } +}; + +/** + * @brief Generate the MJCF of a robot, standing at the origin. + * + * @note The motors are general actuators whose control is the voltage across + * the winding: the gain is the stall torque per volt at the wheel and the + * velocity bias is the back-EMF damping at the wheel, so MuJoCo integrates + * the electrical part implicitly. The rotor inertia reflected through the + * gears is the wheel joint's armature, and the brushes' friction, the + * no-load current times the torque constant, its friction loss. The fan's + * downforce is a force actuator pulling straight down, its reference a + * site fixed in the world, so the board tilted onto its nose adds no + * backward pull that the frictionless skids could not hold. + * + * @param robot The description. + * @return The MJCF text. + */ +std::string robot_mjcf(const RobotDescription& robot); +} // namespace micras::sim + +#endif // MICRAS_SIM_ROBOT_ROBOT_MODEL_HPP diff --git a/engine/include/micras/sim/scenario/scenario.hpp b/engine/include/micras/sim/scenario/scenario.hpp new file mode 100644 index 0000000..ea14d89 --- /dev/null +++ b/engine/include/micras/sim/scenario/scenario.hpp @@ -0,0 +1,219 @@ +/** + * @file + * + * @brief A scenario file: the arena, the duration, and what happens to the robot when. + */ + +#ifndef MICRAS_SIM_SCENARIO_SCENARIO_HPP +#define MICRAS_SIM_SCENARIO_SCENARIO_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/devices/device.hpp" +#include "micras/sim/devices/digital_input.hpp" + +namespace micras::sim { +/** + * @brief One thing that happens to the robot at an instant. + * + * @note With a condition (when and equals) the event waits, from its time on, for the firmware + * variable to take that value, and happens on the first tick it does. Events happen in order, so a + * waiting event holds back the ones after it. + */ +struct ScenarioEvent { + /** + * @brief What kind of thing. + */ + enum class Kind : uint8_t { + PRESS, + SET, + SEND, + }; + + Kind kind{Kind::PRESS}; + double at{0.0}; + double duration{0.0}; + std::string target; + bool value{false}; + std::string when; + std::string equals; +}; + +/** + * @brief When a run ends before its duration: a firmware variable reaching one of some values. + * + * @note Only what happens from `after` on counts, and the run ends the `count`-th time the + * variable reaches one of the values. A value it already holds at `after` counts as reached. + * Any of the abort values ends the run the first time, whatever the count. + */ +struct StopCondition { + std::string variable; + std::vector equals; + double after{0.0}; + int count{1}; + std::vector abort; +}; + +/** + * @brief A scenario, as its TOML file states it. + * + * @note The file names the arena, the duration, the seed and optionally the + * robot and its start pose; then a timeline of events, and optionally a + * stop condition. Events act on what the robot target exposes: its named + * inputs (press, set) and its named link messages (send). Command line + * options override the file's arena, duration and seed. + */ +struct Scenario { + std::string robot; + std::string arena; + double seconds{0.0}; + std::optional seed; + std::optional start; + std::vector events; + std::optional stop; + + /** + * @brief Read a scenario file. + * + * @param path Path of the file. + * @return The scenario. + */ + static Scenario load(const std::filesystem::path& path); + + /** + * @brief Read a scenario. + * + * @param text Contents of a scenario file. + * @param origin Name of the text, for messages. + * @return The scenario. + */ + static Scenario parse(std::string_view text, const std::string& origin = "scenario"); +}; + +/** + * @brief What a robot target exposes to scenarios. + */ +struct ScenarioHooks { + /** + * @brief Inputs a scenario presses or sets, by name. + */ + std::map> inputs; + + /** + * @brief Messages a scenario sends on the link, by name. + */ + std::map, std::less<>> messages; + + /** + * @brief Names of the values of state variables, for conditions and the event log. + */ + StateNames state_names; +}; + +/** + * @brief Plays a scenario's timeline into the run and stops it when its condition holds. + * + * @note Writes inputs and the link only, never the physics state or the clock, + * so a scripted run is a run a human could have driven by hand. Once a + * human takes the board over from the panel it stops touching inputs. + */ +class ScenarioPlayer : public IRunListener { +public: + /** + * @brief Take the scenario and everything it acts on. + * + * @param scenario The scenario. + * @param hooks What the robot exposes. + * @param serial The link's world side. + * @param variables Where stop conditions read, or null. + */ + ScenarioPlayer(Scenario scenario, const ScenarioHooks& hooks, SerialBus& serial, const VariableSource* variables); + + /** + * @brief Apply the events of this tick and check the stop condition. + * + * @param simulation Run about to advance. + * @return QUIT once the stop condition holds. + */ + RunControl on_before_tick(const Simulation& simulation) override; + + /** + * @brief Stop touching inputs: a human has the board. + */ + void hand_over() { this->human = true; } + + /** + * @brief Get the instant the stop condition held. + * + * @return Seconds, or nothing when it never did. + */ + std::optional stopped_at() const { return this->stop_time; } + +private: + /** + * @brief An input to release at a tick. + */ + struct Release { + DigitalInput* input; + uint64_t tick; + }; + + /** + * @brief Check an event against what the robot exposes. + * + * @param event The event. + */ + void check(const ScenarioEvent& event) const; + + /** + * @brief Check whether an event's condition holds. + * + * @param index Index of the event. + * @return True for an event without a condition, or when its variable has the value. + */ + bool is_due(std::size_t index) const; + + /** + * @brief Check the stop condition. + * + * @param now Simulated time in seconds. + * @return QUIT once the condition is reached for the count-th time or takes an abort value. + */ + RunControl check_stop(double now); + + /** + * @brief Turn one of the stop condition's values into the number the variable takes. + * + * @param variable The variable the condition watches. + * @param value A number, or the name of one of the variable's states. + * @return The number. + */ + double resolve(const std::string& variable, const std::string& value) const; + + Scenario scenario; + const ScenarioHooks& hooks; // NOLINT(*-avoid-const-or-ref-data-members): the target outlives the run. + SerialBus& serial; // NOLINT(*-avoid-const-or-ref-data-members): the run's bus. + const VariableSource* variables; + std::vector releases; + std::size_t next_event{0}; + std::vector stop_values; + std::vector event_values; + std::vector abort_values; + bool stop_matching{false}; + int stop_reached{0}; + std::optional stop_time; + bool human{false}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_SCENARIO_SCENARIO_HPP diff --git a/engine/src/.clang-tidy b/engine/src/.clang-tidy new file mode 100644 index 0000000..9ab6220 --- /dev/null +++ b/engine/src/.clang-tidy @@ -0,0 +1,9 @@ +--- +# toml++ declares what it provides in headers of its own implementation, reached through toml.hpp, +# which is the one a user of the library includes. Its table's operator[] is a lookup that yields an +# empty view for a missing key, like a map's, so it is not an unchecked access. +InheritParentConfig: true +CheckOptions: + misc-include-cleaner.IgnoreHeaders: 'toml\+\+/.*' + cppcoreguidelines-pro-bounds-avoid-unchecked-container-access.ExcludeClasses: '::std::map;::std::unordered_map;::std::flat_map;::toml::table;::toml::node_view' +... diff --git a/src/sim/core/clock.cpp b/engine/src/core/clock.cpp similarity index 78% rename from src/sim/core/clock.cpp rename to engine/src/core/clock.cpp index 0e8d0cf..92c7f84 100644 --- a/src/sim/core/clock.cpp +++ b/engine/src/core/clock.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include @@ -44,25 +45,11 @@ void Clock::advance() { this->ticks++; } -void Clock::reset() { - this->elapsed_us = 0; - this->ticks = 0; -} - -uint64_t Clock::total_ticks(double seconds, uint32_t us_per_tick) { - return static_cast(seconds * 1e6 / us_per_tick); -} - uint64_t Clock::total_ticks(double seconds) const { - return total_ticks(seconds, this->tick_us); + return static_cast(seconds * 1e6 / this->tick_us); } uint64_t Clock::tick_at(double seconds) const { return static_cast(std::llround(seconds * 1e6 / this->tick_us)); } - -uint64_t Clock::ticks_for_elapsed_ms(uint32_t target_ms) const { - const uint64_t target_us = static_cast(target_ms) * 1000; - return (target_us + this->tick_us - 1) / this->tick_us; -} } // namespace micras::sim diff --git a/src/sim/core/firmware_thread.cpp b/engine/src/core/firmware_thread.cpp similarity index 91% rename from src/sim/core/firmware_thread.cpp rename to engine/src/core/firmware_thread.cpp index 1d87a38..f2f9e87 100644 --- a/src/sim/core/firmware_thread.cpp +++ b/engine/src/core/firmware_thread.cpp @@ -4,8 +4,11 @@ #include #include +#include #include +#include #include +#include #include #include "micras/sim/core/firmware_thread.hpp" @@ -75,12 +78,12 @@ void FirmwareThread::finish() { } bool FirmwareThread::has_finished() const { - const std::lock_guard lock(this->mutex); + const std::scoped_lock lock(this->mutex); return this->finished; } void FirmwareThread::rethrow_any_error() const { - const std::lock_guard lock(this->mutex); + const std::scoped_lock lock(this->mutex); if (this->error) { std::rethrow_exception(this->error); @@ -108,14 +111,14 @@ void FirmwareThread::thread_body() { } try { - this->program(); + this->program(*this); } catch (const RunFinished&) { // NOLINT(bugprone-empty-catch): the expected way out, see finish(). } catch (...) { - const std::lock_guard lock(this->mutex); + const std::scoped_lock lock(this->mutex); this->error = std::current_exception(); } - const std::lock_guard lock(this->mutex); + const std::scoped_lock lock(this->mutex); this->finished = true; this->turn = Turn::SIMULATION; this->handoff.notify_all(); diff --git a/engine/src/core/mujoco_world.cpp b/engine/src/core/mujoco_world.cpp new file mode 100644 index 0000000..fc61b4a --- /dev/null +++ b/engine/src/core/mujoco_world.cpp @@ -0,0 +1,369 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/span_at.hpp" + +namespace micras::sim { +/** + * @brief Check an index against the array it is about to reach into. + * + * @param index Index to check. + * @param size Size of the array. + * @param what Name of the array, for the error message. + * @return The index, as an unsigned offset. + */ +static std::size_t require_index(int index, std::size_t size, const char* what) { + if (index < 0 or std::cmp_greater_equal(index, size)) { + throw std::out_of_range( + std::string(what) + " index " + std::to_string(index) + " is outside the " + std::to_string(size) + + " entries the model has" + ); + } + + return static_cast(index); +} + +static std::array + point_velocity(const mjModel* model, const mjData* data, int geom, const std::array& point) { + std::array velocity{}; + mj_objectVelocity(model, data, mjOBJ_GEOM, geom, velocity.data(), 0); + + const std::span centers(data->geom_xpos, 3 * static_cast(model->ngeom)); + const std::size_t base = 3 * static_cast(geom); + const std::array arm{ + point.at(0) - at(centers, base), point.at(1) - at(centers, base + 1), point.at(2) - at(centers, base + 2) + }; + + return { + velocity.at(3) + (velocity.at(1) * arm.at(2)) - (velocity.at(2) * arm.at(1)), + velocity.at(4) + (velocity.at(2) * arm.at(0)) - (velocity.at(0) * arm.at(2)), + velocity.at(5) + (velocity.at(0) * arm.at(1)) - (velocity.at(1) * arm.at(0)), + }; +} + +static std::array sliding_velocity( + const mjModel* model, const mjData* data, const mjContact& contact, int geom, int other, + std::array& point +) { + const std::span position(contact.pos); + const std::span frame(contact.frame); + std::ranges::copy(position, point.begin()); + + const std::array own = point_velocity(model, data, geom, point); + const std::array theirs = point_velocity(model, data, other, point); + + std::array sliding{}; + mjtNum along = 0.0; + + for (std::size_t axis = 0; axis < 3; axis++) { + sliding.at(axis) = own.at(axis) - theirs.at(axis); + along += sliding.at(axis) * at(frame, axis); + } + + for (std::size_t axis = 0; axis < 3; axis++) { + sliding.at(axis) -= along * at(frame, axis); + } + + return sliding; +} + +MujocoWorld::~MujocoWorld() { + this->unload(); +} + +void MujocoWorld::load(const std::filesystem::path& model_path) { + std::array error{}; + mjModel* loaded = mj_loadXML(model_path.c_str(), nullptr, error.data(), static_cast(error.size())); + + if (loaded == nullptr) { + throw std::runtime_error("failed to load model '" + model_path.string() + "': " + std::string(error.data())); + } + + mjData* allocated = mj_makeData(loaded); + + if (allocated == nullptr) { + mj_deleteModel(loaded); + throw std::runtime_error("failed to allocate mjData for '" + model_path.string() + "'"); + } + + this->unload(); + this->model_handle = loaded; + this->data_handle = allocated; + this->model_handle->opt.disableflags |= mjDSBL_AUTORESET; +} + +void MujocoWorld::build( + const std::string& robot_xml, std::string_view robot_body, const std::string& arena_xml, + std::string_view arena_body, const Placement& placement +) { + std::array error{}; + const auto fail = [&error](const std::string& what) { + return std::runtime_error(what + ": " + std::string(error.data())); + }; + + const std::unique_ptr robot{ + mj_parseXMLString(robot_xml.c_str(), nullptr, error.data(), static_cast(error.size())), mj_deleteSpec + }; + + if (robot == nullptr) { + throw fail("the robot's MJCF does not parse"); + } + + const std::unique_ptr arena{ + mj_parseXMLString(arena_xml.c_str(), nullptr, error.data(), static_cast(error.size())), mj_deleteSpec + }; + + if (arena == nullptr) { + throw fail("the arena's MJCF does not parse"); + } + + const mjsBody* const arena_root = mjs_findBody(arena.get(), std::string{arena_body}.c_str()); + mjsBody* const robot_root = mjs_findBody(robot.get(), std::string{robot_body}.c_str()); + + if (arena_root == nullptr or robot_root == nullptr) { + throw std::runtime_error("the arena or the robot does not have the body it was asked to attach or place"); + } + + const mjsFrame* const frame = mjs_addFrame(mjs_findBody(robot.get(), "world"), nullptr); + const std::string prefix = std::string{arena_body} + "_"; + const mjsElement* attached = mjs_attach(frame->element, arena_root->element, prefix.c_str(), ""); + + if (attached == nullptr) { + throw std::runtime_error(std::string("the arena cannot be attached: ") + mjs_getError(robot.get())); + } + + robot_root->pos[0] = placement.x; + robot_root->pos[1] = placement.y; + robot_root->pos[2] = 0.0; + robot_root->quat[0] = std::cos(placement.yaw / 2); + robot_root->quat[1] = 0.0; + robot_root->quat[2] = 0.0; + robot_root->quat[3] = std::sin(placement.yaw / 2); + + mjModel* const compiled = mj_compile(robot.get(), nullptr); + + if (compiled == nullptr) { + throw std::runtime_error(std::string("the composed model does not compile: ") + mjs_getError(robot.get())); + } + + std::vector xml(1 << 22); + + if (mj_saveXMLString( + robot.get(), xml.data(), static_cast(xml.size()), error.data(), static_cast(error.size()) + ) != 0) { + mj_deleteModel(compiled); + throw fail("the composed model cannot be written back"); + } + + mjData* const allocated = mj_makeData(compiled); + + if (allocated == nullptr) { + mj_deleteModel(compiled); + throw std::runtime_error("failed to allocate mjData for the composed model"); + } + + this->unload(); + this->model_handle = compiled; + this->data_handle = allocated; + this->model_handle->opt.disableflags |= mjDSBL_AUTORESET; + this->composed = xml.data(); +} + +void MujocoWorld::reset() { + this->require_loaded(); + mj_resetData(this->model_handle, this->data_handle); + mj_forward(this->model_handle, this->data_handle); +} + +void MujocoWorld::step(int steps) { + this->require_loaded(); + + for (int i = 0; i < steps; i++) { + this->apply_sliding_friction(); + mj_step(this->model_handle, this->data_handle); + } +} + +void MujocoWorld::add_sliding_friction(const std::string& geom, double coefficient) { + const int id = this->require_id(mjOBJ_GEOM, geom); + const std::span geom_body( + this->model_handle->geom_bodyid, static_cast(this->model_handle->ngeom) + ); + + this->sliding_frictions.push_back( + {.geom = id, .body = at(geom_body, static_cast(id)), .coefficient = coefficient} + ); +} + +void MujocoWorld::apply_sliding_friction() { + if (this->sliding_frictions.empty()) { + return; + } + + const mjModel* model = this->model_handle; + mjData* data = this->data_handle; + + const std::span applied(data->qfrc_applied, static_cast(model->nv)); + const std::span contacts(data->contact, static_cast(data->ncon)); + const std::span geom_body(model->geom_bodyid, static_cast(model->ngeom)); + + std::ranges::fill(applied, 0.0); + + for (const SlidingFriction& friction : this->sliding_frictions) { + for (std::size_t i = 0; i < contacts.size(); i++) { + const mjContact& contact = at(contacts, i); + + if (contact.geom1 != friction.geom and contact.geom2 != friction.geom) { + continue; + } + + std::array force{}; + mj_contactForce(model, data, static_cast(i), force.data()); + + if (force.at(0) <= 0.0) { + continue; + } + + const int other = contact.geom1 == friction.geom ? contact.geom2 : contact.geom1; + std::array where{}; + const std::array sliding = sliding_velocity(model, data, contact, friction.geom, other, where); + const mjtNum speed = std::hypot(sliding.at(0), sliding.at(1), sliding.at(2)); + const mjtNum scale = -friction.coefficient * force.at(0) / std::max(speed, sticking_speed); + std::array push{scale * sliding.at(0), scale * sliding.at(1), scale * sliding.at(2)}; + std::array torque{}; + + mj_applyFT(model, data, push.data(), torque.data(), where.data(), friction.body, applied.data()); + + const int other_body = at(geom_body, static_cast(other)); + + if (other_body != 0) { + for (mjtNum& component : push) { + component = -component; + } + + mj_applyFT(model, data, push.data(), torque.data(), where.data(), other_body, applied.data()); + } + } + } +} + +int MujocoWorld::require_id(mjtObj type, const std::string& name) const { + this->require_loaded(); + const int id = mj_name2id(this->model_handle, type, name.c_str()); + + if (id < 0) { + throw std::runtime_error("model object not found: '" + name + "'"); + } + + return id; +} + +int MujocoWorld::sensor_address(const std::string& name) const { + const int id = this->require_id(mjOBJ_SENSOR, name); + const std::span addresses( + this->model_handle->sensor_adr, static_cast(this->model_handle->nsensor) + ); + return at(addresses, static_cast(id)); +} + +int MujocoWorld::joint_dof(const std::string& name) const { + const int id = this->require_id(mjOBJ_JOINT, name); + const std::span addresses( + this->model_handle->jnt_dofadr, static_cast(this->model_handle->njnt) + ); + return at(addresses, static_cast(id)); +} + +int MujocoWorld::joint_qpos(const std::string& name) const { + const int id = this->require_id(mjOBJ_JOINT, name); + const std::span addresses( + this->model_handle->jnt_qposadr, static_cast(this->model_handle->njnt) + ); + return at(addresses, static_cast(id)); +} + +double MujocoWorld::sensor_value(int address) const { + this->require_loaded(); + const std::span readings( + this->data_handle->sensordata, static_cast(this->model_handle->nsensordata) + ); + return at(readings, require_index(address, readings.size(), "sensordata")); +} + +void MujocoWorld::set_control(int actuator_id, double value) { + this->require_loaded(); + const std::span controls(this->data_handle->ctrl, static_cast(this->model_handle->nu)); + at(controls, require_index(actuator_id, controls.size(), "actuator")) = value; +} + +double MujocoWorld::timestep() const { + this->require_loaded(); + return this->model_handle->opt.timestep; +} + +double MujocoWorld::time() const { + this->require_loaded(); + return this->data_handle->time; +} + +const mjModel* MujocoWorld::model() const { + this->require_loaded(); + return this->model_handle; +} + +mjModel* MujocoWorld::model() { + this->require_loaded(); + return this->model_handle; +} + +const mjData* MujocoWorld::data() const { + this->require_loaded(); + return this->data_handle; +} + +mjData* MujocoWorld::data() { + this->require_loaded(); + return this->data_handle; +} + +void MujocoWorld::require_loaded() const { + if (this->model_handle == nullptr or this->data_handle == nullptr) { + throw std::logic_error("the MuJoCo world was used before a model was loaded"); + } +} + +void MujocoWorld::unload() { + this->sliding_frictions.clear(); + + if (this->data_handle != nullptr) { + mj_deleteData(this->data_handle); + this->data_handle = nullptr; + } + + if (this->model_handle != nullptr) { + mj_deleteModel(this->model_handle); + this->model_handle = nullptr; + } +} +} // namespace micras::sim diff --git a/engine/src/core/noise.cpp b/engine/src/core/noise.cpp new file mode 100644 index 0000000..82975fc --- /dev/null +++ b/engine/src/core/noise.cpp @@ -0,0 +1,54 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/sim/core/noise.hpp" + +namespace micras::sim { +/** + * @brief Mix a seed with a name, FNV-1a over the name's bytes. + * + * @param seed The run's seed. + * @param stream Name of the stream. + * @return The stream's seed. + */ +static uint64_t mix(uint64_t seed, std::string_view stream) { + uint64_t hash = 0xcbf29ce484222325ULL ^ seed; + + for (const char character : stream) { + hash ^= static_cast(character); + hash *= 0x100000001b3ULL; + } + + return hash; +} + +Noise::Noise(const NoiseConfig& config, std::string_view stream) : + engine{mix(config.seed, stream)}, on{not config.ideal} { } + +double Noise::unit() { + return (static_cast(this->engine() >> 11U) + 1.0) * 0x1.0p-53; +} + +double Noise::gaussian(double sigma) { + if (not this->on or sigma == 0.0) { + return 0.0; + } + + if (this->has_spare) { + this->has_spare = false; + return sigma * this->spare; + } + + const double radius = std::sqrt(-2.0 * std::log(this->unit())); + const double angle = 2.0 * std::numbers::pi * this->unit(); + this->spare = radius * std::sin(angle); + this->has_spare = true; + return sigma * radius * std::cos(angle); +} +} // namespace micras::sim diff --git a/engine/src/core/serial_bus.cpp b/engine/src/core/serial_bus.cpp new file mode 100644 index 0000000..a4e365c --- /dev/null +++ b/engine/src/core/serial_bus.cpp @@ -0,0 +1,49 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include + +#include "micras/sim/core/serial_bus.hpp" + +namespace micras::sim { +void SerialBus::add_listener(ISerialListener& listener) { + if (std::ranges::find(this->listeners, &listener) == this->listeners.end()) { + this->listeners.push_back(&listener); + } +} + +void SerialBus::remove_listener(const ISerialListener& listener) { + std::erase(this->listeners, &listener); +} + +void SerialBus::send_from_firmware(std::span bytes) { + for (ISerialListener* listener : this->listeners) { + listener->on_firmware_bytes(bytes); + } +} + +void SerialBus::queue_for_firmware(std::span bytes) { + for (const uint8_t byte : bytes) { + if (this->to_firmware.size() >= max_pending_bytes) { + this->dropped++; + continue; + } + + this->to_firmware.push_back(byte); + } +} + +std::vector SerialBus::take_for_firmware(std::size_t count) { + const std::size_t taken = std::min(count, this->to_firmware.size()); + std::vector bytes( + this->to_firmware.begin(), this->to_firmware.begin() + static_cast(taken) + ); + this->to_firmware.erase(this->to_firmware.begin(), this->to_firmware.begin() + static_cast(taken)); + return bytes; +} +} // namespace micras::sim diff --git a/src/sim/core/simulation.cpp b/engine/src/core/simulation.cpp similarity index 83% rename from src/sim/core/simulation.cpp rename to engine/src/core/simulation.cpp index a2384e2..f06932e 100644 --- a/src/sim/core/simulation.cpp +++ b/engine/src/core/simulation.cpp @@ -2,6 +2,10 @@ * @file */ +#include + +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" #include "micras/sim/core/simulation.hpp" namespace micras::sim { @@ -38,8 +42,7 @@ void IRunListener::on_after_tick(const Simulation& /*simulation*/) { } void IRunListener::on_finish(const Simulation& /*simulation*/) { } -Simulation::Simulation(SimulationContext& context, FirmwareThread& firmware) : - simulation{context}, firmware{firmware} { } +Simulation::Simulation(RunContext& context, FirmwareThread& firmware) : simulation{context}, firmware{firmware} { } void Simulation::add_listener(IRunListener& listener) { this->listeners.push_back(&listener); @@ -91,7 +94,15 @@ RunControl Simulation::before_tick() { } void Simulation::step() { + for (const auto& device : this->simulation.devices) { + device->actuate(this->simulation.world, this->simulation.clock); + } + this->simulation.world.step(this->simulation.clock.steps_per_tick()); this->simulation.clock.advance(); + + for (const auto& device : this->simulation.devices) { + device->sample(this->simulation.world, this->simulation.clock); + } } } // namespace micras::sim diff --git a/engine/src/devices/dc_motor.cpp b/engine/src/devices/dc_motor.cpp new file mode 100644 index 0000000..70ace40 --- /dev/null +++ b/engine/src/devices/dc_motor.cpp @@ -0,0 +1,52 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/devices/dc_motor.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +DcMotor::DcMotor(const MujocoWorld& world, Config config) : + config{std::move(config)}, + actuator_id{world.require_id(mjOBJ_ACTUATOR, this->config.actuator)}, + velocity_address{world.joint_dof(this->config.joint)} { } + +void DcMotor::actuate(MujocoWorld& world, const Clock& /*clock*/) { + const DriveDescription& drive = this->config.drive; + const std::span velocities(world.data()->qvel, static_cast(world.model()->nv)); + const double back_emf = + drive.speed_constant * drive.gear_ratio * at(velocities, static_cast(this->velocity_address)); + + if (this->config.enabled()) { + const double duty = static_cast(this->config.forward_duty() - this->config.backward_duty()) / 100.0; + this->voltage = std::clamp(duty, -1.0, 1.0) * drive.supply_voltage; + } else { + this->voltage = back_emf; + } + + this->winding_current = (this->voltage - back_emf) / drive.resistance(); + world.set_control(this->actuator_id, this->voltage); +} + +std::vector DcMotor::columns() const { + return {this->config.name + "_voltage", this->config.name + "_current"}; +} + +void DcMotor::append(std::vector& row) const { + row.emplace_back(this->voltage); + row.emplace_back(this->winding_current); +} +} // namespace micras::sim diff --git a/engine/src/devices/device.cpp b/engine/src/devices/device.cpp new file mode 100644 index 0000000..6262c61 --- /dev/null +++ b/engine/src/devices/device.cpp @@ -0,0 +1,23 @@ +/** + * @file + */ + +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/devices/device.hpp" +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +void Device::actuate(MujocoWorld& /*world*/, const Clock& /*clock*/) { } + +void Device::sample(MujocoWorld& /*world*/, const Clock& /*clock*/) { } + +std::vector Device::columns() const { + return {}; +} + +void Device::append(std::vector& /*row*/) const { } +} // namespace micras::sim diff --git a/engine/src/devices/digital_input.cpp b/engine/src/devices/digital_input.cpp new file mode 100644 index 0000000..592a0a7 --- /dev/null +++ b/engine/src/devices/digital_input.cpp @@ -0,0 +1,30 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/sim/devices/digital_input.hpp" +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +DigitalInput::DigitalInput(Config config) : config{std::move(config)} { + this->set(false); +} + +void DigitalInput::set(bool active) { + this->active = active; + this->config.drive(active != this->config.active_low); +} + +std::vector DigitalInput::columns() const { + return {this->config.name}; +} + +void DigitalInput::append(std::vector& row) const { + row.emplace_back(static_cast(this->active)); +} +} // namespace micras::sim diff --git a/engine/src/devices/imu.cpp b/engine/src/devices/imu.cpp new file mode 100644 index 0000000..c2fb5d4 --- /dev/null +++ b/engine/src/devices/imu.cpp @@ -0,0 +1,106 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/noise.hpp" +#include "micras/sim/devices/imu.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Largest and smallest raw word of a 16-bit output register. + */ +///@{ +constexpr double word_max{32767.0}; +constexpr double word_min{-32768.0}; +///@} +} // namespace + +Imu::Imu(const MujocoWorld& world, Config config, const NoiseConfig& noise) : + config{std::move(config)}, + noise{noise, this->config.name}, + gyro_address{world.sensor_address(this->config.gyro)}, + accelerometer_address{world.sensor_address(this->config.accelerometer)} { + const ImuDescription& chip = this->config.description; + + for (std::size_t axis = 0; axis < 3; axis++) { + this->bias.at(axis) = this->noise.gaussian(chip.gyro_bias); + this->bias.at(3 + axis) = this->noise.gaussian(chip.accel_bias); + this->scale.at(axis) = 1.0 + this->noise.gaussian(chip.gyro_scale_error); + this->scale.at(3 + axis) = 1.0; + } + + this->rate_error = this->noise.gaussian(chip.clock_error); +} + +void Imu::sample(MujocoWorld& world, const Clock& clock) { + const ImuDescription& chip = this->config.description; + const double period = clock.us_per_tick() * 1e-6; + const double alpha = 1.0 - std::exp(-2.0 * std::numbers::pi * chip.bandwidth * period); + const double bandwidth = std::numbers::pi / 2.0 * chip.bandwidth; + const std::array sigma{ + chip.gyro_noise_density * std::sqrt(bandwidth), chip.accel_noise_density * std::sqrt(bandwidth) + }; + const std::array resolution{chip.gyro_resolution, chip.accel_resolution}; + + for (std::size_t channel = 0; channel < channels; channel++) { + const int address = channel < 3 ? this->gyro_address : this->accelerometer_address; + const double truth = world.sensor_value(address + static_cast(channel % 3)); + this->filtered.at(channel) = + this->started ? this->filtered.at(channel) + alpha * (truth - this->filtered.at(channel)) : truth; + } + + this->started = true; + this->phase += chip.output_rate * (1.0 + this->rate_error) * period; + + while (this->phase >= 1.0) { + this->phase -= 1.0; + + if (this->has_pending) { + this->delivered = this->pending; + this->config.write(this->delivered); + } + + for (std::size_t channel = 0; channel < channels; channel++) { + const std::size_t kind = channel / 3; + const double value = this->scale.at(channel) * this->filtered.at(channel) + this->bias.at(channel) + + this->noise.gaussian(sigma.at(kind)); + this->pending.at(channel) = + static_cast(std::clamp(std::round(value / resolution.at(kind)), word_min, word_max)); + } + + this->has_pending = true; + } +} + +std::vector Imu::columns() const { + const std::string& name = this->config.name; + return {name + "_gyro_x", name + "_gyro_y", name + "_gyro_z", + name + "_accel_x", name + "_accel_y", name + "_accel_z"}; +} + +void Imu::append(std::vector& row) const { + const ImuDescription& chip = this->config.description; + + for (std::size_t channel = 0; channel < channels; channel++) { + row.emplace_back( + static_cast(this->delivered.at(channel)) * + (channel < 3 ? chip.gyro_resolution : chip.accel_resolution) + ); + } +} +} // namespace micras::sim diff --git a/engine/src/devices/power.cpp b/engine/src/devices/power.cpp new file mode 100644 index 0000000..6d96c02 --- /dev/null +++ b/engine/src/devices/power.cpp @@ -0,0 +1,97 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/noise.hpp" +#include "micras/sim/devices/power.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +/** + * @brief Convert a voltage to ADC counts. + * + * @param voltage Voltage at the ADC input. + * @param reference ADC reference voltage. + * @param max_counts Counts at the reference. + * @param noise Noise added, in counts. + * @return Counts, clipped to the ADC's range. + */ +static uint32_t to_counts(double voltage, double reference, double max_counts, double noise) { + return static_cast(std::clamp(std::round(voltage / reference * max_counts + noise), 0.0, max_counts)); +} + +Battery::Battery(Config config, const NoiseConfig& noise) : + config{std::move(config)}, + noise{noise, this->config.name}, + terminal_voltage{this->config.description.cells * this->config.description.cell_voltage} { } + +void Battery::sample(MujocoWorld& /*world*/, const Clock& /*clock*/) { + this->config.write(to_counts( + this->terminal_voltage / this->config.divider, this->config.adc_reference, this->config.adc_max_counts, + this->noise.gaussian(this->config.adc_noise_counts) + )); +} + +std::vector Battery::columns() const { + return {this->config.name + "_voltage"}; +} + +void Battery::append(std::vector& row) const { + row.emplace_back(this->terminal_voltage); +} + +Fan::Fan(const MujocoWorld& world, Config config) : + config{std::move(config)}, actuator_id{world.require_id(mjOBJ_ACTUATOR, this->config.actuator)} { } + +void Fan::actuate(MujocoWorld& world, const Clock& clock) { + const FanDescription& fan = this->config.description; + double target = 0.0; + + if (this->config.enabled()) { + const double speed = std::clamp(static_cast(this->config.duty()), 0.0, 100.0) / 100.0 * + this->config.supply_voltage() / fan.nominal_voltage; + target = fan.max_downforce * speed * speed; + } + + const double period = clock.us_per_tick() * 1e-6; + const double blend = 1.0 - std::exp(-period / fan.time_constant); + this->downforce += blend * (target - this->downforce); + world.set_control(this->actuator_id, this->downforce); +} + +std::vector Fan::columns() const { + return {this->config.name + "_downforce"}; +} + +void Fan::append(std::vector& row) const { + row.emplace_back(this->downforce); +} + +CurrentSense::CurrentSense(Config config, const NoiseConfig& noise) : + config{std::move(config)}, noise{noise, "current_sense"} { } + +void CurrentSense::sample(MujocoWorld& /*world*/, const Clock& /*clock*/) { + for (std::size_t channel = 0; channel < this->config.currents.size(); channel++) { + const double current = this->config.currents.at(channel)(); + this->config.write( + channel, to_counts( + this->config.zero_voltage + current * this->config.volts_per_amp, this->config.adc_reference, + this->config.adc_max_counts, this->noise.gaussian(this->config.adc_noise_counts) + ) + ); + } +} +} // namespace micras::sim diff --git a/engine/src/devices/quadrature_encoder.cpp b/engine/src/devices/quadrature_encoder.cpp new file mode 100644 index 0000000..8b1635d --- /dev/null +++ b/engine/src/devices/quadrature_encoder.cpp @@ -0,0 +1,42 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/devices/quadrature_encoder.hpp" +#include "micras/sim/recording/csv_writer.hpp" + +namespace micras::sim { +QuadratureEncoder::QuadratureEncoder(const MujocoWorld& world, Config config) : + config{std::move(config)}, position_address{world.joint_qpos(this->config.joint)} { } + +void QuadratureEncoder::sample(MujocoWorld& world, const Clock& /*clock*/) { + const std::span positions(world.data()->qpos, static_cast(world.model()->nq)); + const double angle = at(positions, static_cast(this->position_address)); + + this->count = + static_cast(std::floor(angle * this->config.counts_per_revolution / (2.0 * std::numbers::pi))); + this->config.write(this->count); +} + +std::vector QuadratureEncoder::columns() const { + return {this->config.name + "_count"}; +} + +void QuadratureEncoder::append(std::vector& row) const { + row.emplace_back(static_cast(this->count)); +} +} // namespace micras::sim diff --git a/engine/src/devices/serial_link.cpp b/engine/src/devices/serial_link.cpp new file mode 100644 index 0000000..8409439 --- /dev/null +++ b/engine/src/devices/serial_link.cpp @@ -0,0 +1,57 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/devices/serial_link.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Bit times per byte on an 8N1 line. + */ +constexpr double bits_per_byte{10.0}; +} // namespace + +SerialLink::SerialLink(SerialBus& bus, Config config) : bus{bus}, config{std::move(config)} { } + +void SerialLink::sample(MujocoWorld& /*world*/, const Clock& clock) { + const double bytes_per_tick = this->config.baud_rate / bits_per_byte * clock.us_per_tick() * 1e-6; + + this->send_budget = std::min(this->send_budget + bytes_per_tick, 1.0 + bytes_per_tick); + std::vector sent; + + while (this->send_budget >= 1.0) { + const std::optional byte = this->config.take_sent(); + + if (not byte.has_value()) { + break; + } + + sent.push_back(*byte); + this->send_budget -= 1.0; + } + + if (not sent.empty()) { + this->bus.send_from_firmware(sent); + } + + this->receive_budget = std::min(this->receive_budget + bytes_per_tick, 1.0 + bytes_per_tick); + const auto count = static_cast(std::floor(this->receive_budget)); + + for (const uint8_t byte : this->bus.take_for_firmware(count)) { + this->config.receive(byte); + this->receive_budget -= 1.0; + } +} +} // namespace micras::sim diff --git a/engine/src/devices/wall_sensors.cpp b/engine/src/devices/wall_sensors.cpp new file mode 100644 index 0000000..4d8d5ed --- /dev/null +++ b/engine/src/devices/wall_sensors.cpp @@ -0,0 +1,252 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/noise.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/devices/wall_sensors.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Angle, in half angles, the cone is traced out to. + */ +constexpr double cone_extent{2.5}; +} // namespace + +/** + * @brief Relative intensity of an emitter or sensitivity of a receiver off its axis. + * + * @param angle Angle off the axis. + * @param half_angle Angle at which it halves. + * @return Factor between 0 and 1. + */ +static double lobe(double angle, double half_angle) { + const double ratio = angle / half_angle; + return std::exp2(-ratio * ratio); +} + +/** + * @brief Lay out a cone of rays: one on the axis and rings around it. + * + * @param count Number of rays, one plus a multiple of the rings. + * @param half_angle Emitter half angle. + * @param intensity Radiant intensity on the axis, in W/sr. + * @return The rays, each carrying the flux of its patch of the cone. + */ +static std::vector> cone(int count, double half_angle, double intensity) { + constexpr int rings = 3; + const int per_ring = std::max(1, (count - 1) / rings); + const double step = cone_extent * half_angle / (rings + 0.5); + std::vector> rays; + + const double center_solid_angle = 2.0 * std::numbers::pi * (1.0 - std::cos(step / 2)); + rays.push_back({1.0, 0.0, 0.0, intensity * center_solid_angle}); + + for (int ring = 1; ring <= rings; ring++) { + const double angle = ring * step; + const double solid_angle = + 2.0 * std::numbers::pi * (std::cos(angle - step / 2) - std::cos(angle + step / 2)) / per_ring; + + for (int index = 0; index < per_ring; index++) { + const double around = 2.0 * std::numbers::pi * (index + 0.5 * (ring % 2)) / per_ring; + rays.push_back( + {std::cos(angle), std::sin(angle) * std::cos(around), std::sin(angle) * std::sin(around), + intensity * lobe(angle, half_angle) * solid_angle} + ); + } + } + + return rays; +} + +WallSensors::WallSensors(const MujocoWorld& world, Config config, const NoiseConfig& noise) : + config{std::move(config)}, noise{noise, this->config.name} { + const WallSensorsDescription& optics = this->config.description; + + for (const WallSensorDescription& sensor : optics.sensors) { + this->emitter_sites.push_back(world.require_id(mjOBJ_SITE, sensor.name + "_emitter")); + this->receiver_sites.push_back(world.require_id(mjOBJ_SITE, sensor.name + "_receiver")); + } + + for (const auto& ray : cone(optics.rays, optics.emitter_half_angle, optics.emitter_intensity)) { + this->rays.push_back({.direction = {ray.at(0), ray.at(1), ray.at(2)}, .flux = ray.at(3)}); + } + + std::set group_ids; + + for (const WallSensorDescription& sensor : optics.sensors) { + group_ids.insert(sensor.group); + } + + if (group_ids.size() != this->groups.size()) { + throw std::runtime_error(this->config.name + ": the emitters must fire in two groups"); + } + + std::ranges::copy(group_ids, this->groups.begin()); + std::ranges::fill(this->geom_groups, 1); + this->geom_groups.at(MujocoWorld::unseen_group) = 0; + this->counts.assign(2 * optics.sensors.size(), 0); + this->intensities.assign(optics.sensors.size(), 0.0); + this->directions.resize(3 * this->rays.size()); + this->distances.resize(this->rays.size()); + this->normals.resize(3 * this->rays.size()); + this->geoms.resize(this->rays.size()); +} + +double WallSensors::irradiance(MujocoWorld& world, std::size_t emitter, std::size_t receiver) { + const mjModel* model = world.model(); + mjData* data = world.data(); + const WallSensorsDescription& optics = this->config.description; + const std::span site_positions(data->site_xpos, static_cast(3 * model->nsite)); + const std::span site_frames(data->site_xmat, static_cast(9 * model->nsite)); + const auto emitter_site = static_cast(this->emitter_sites.at(emitter)); + const auto receiver_site = static_cast(this->receiver_sites.at(receiver)); + const std::span origin = site_positions.subspan(3 * emitter_site, 3); + const std::span frame = site_frames.subspan(9 * emitter_site, 9); + const std::span receiver_position = site_positions.subspan(3 * receiver_site, 3); + const std::span receiver_frame = site_frames.subspan(9 * receiver_site, 9); + + for (std::size_t ray = 0; ray < this->rays.size(); ray++) { + const std::array& local = this->rays.at(ray).direction; + + for (std::size_t axis = 0; axis < 3; axis++) { + this->directions.at(3 * ray + axis) = at(frame, 3 * axis) * local.at(0) + + at(frame, 3 * axis + 1) * local.at(1) + + at(frame, 3 * axis + 2) * local.at(2); + } + } + + mj_multiRay( + model, data, origin.data(), this->directions.data(), this->geom_groups.data(), true, -1, this->geoms.data(), + this->distances.data(), this->normals.data(), static_cast(this->rays.size()), cutoff + ); + + double total = 0.0; + + for (std::size_t ray = 0; ray < this->rays.size(); ray++) { + const double distance = this->distances.at(ray); + + if (distance < 0.0 or this->geoms.at(ray) < 0) { + continue; + } + + std::array to_receiver{}; + double length = 0.0; + + for (std::size_t axis = 0; axis < 3; axis++) { + const double hit = at(origin, axis) + distance * this->directions.at(3 * ray + axis); + to_receiver.at(axis) = at(receiver_position, axis) - hit; + length += to_receiver.at(axis) * to_receiver.at(axis); + } + + length = std::sqrt(length); + + if (length <= 0.0) { + continue; + } + + double emission = 0.0; + double incidence = 0.0; + + for (std::size_t axis = 0; axis < 3; axis++) { + emission += this->normals.at(3 * ray + axis) * to_receiver.at(axis) / length; + incidence -= at(receiver_frame, 3 * axis) * to_receiver.at(axis) / length; + } + + if (emission <= 0.0 or incidence <= 0.0) { + continue; + } + + const double off_axis = std::acos(std::min(1.0, incidence)); + total += this->config.reflectance(this->geoms.at(ray)) * this->rays.at(ray).flux * emission * incidence * + lobe(off_axis, optics.receiver_half_angle) / (std::numbers::pi * length * length); + } + + return total; +} + +void WallSensors::sample(MujocoWorld& world, const Clock& clock) { + if (clock.tick_count() % this->config.scan_ticks != 0) { + return; + } + + const WallSensorsDescription& optics = this->config.description; + const std::size_t sensors = optics.sensors.size(); + const std::size_t scan = this->scans % this->groups.size(); + const int lit = this->groups.at(scan); + + for (std::size_t receiver = 0; receiver < sensors; receiver++) { + double signal = 0.0; + + for (std::size_t emitter = 0; emitter < sensors; emitter++) { + if (optics.sensors.at(emitter).group == lit and this->config.emitter_duty(emitter) > 0.0F) { + signal += this->irradiance(world, emitter, receiver); + } + } + + const double current = optics.receiver_responsivity * optics.sensors.at(receiver).gain * + (optics.settle_fraction * signal + optics.ambient_irradiance); + const double voltage = std::min(current * optics.load_resistance, optics.saturation_voltage); + const double reading = + voltage / optics.adc_reference * optics.adc_max_counts + this->noise.gaussian(optics.adc_noise_counts); + const auto counts = static_cast(std::clamp(std::round(reading), 0.0, optics.adc_max_counts)); + + this->counts.at(scan * sensors + receiver) = counts; + this->config.write(scan * sensors + receiver, counts); + } + + this->scans++; + + if (scan + 1 == this->groups.size()) { + for (std::size_t sensor = 0; sensor < sensors; sensor++) { + const std::size_t own = optics.sensors.at(sensor).group == this->groups.at(0) ? 0 : 1; + const std::size_t other = 1 - own; + this->intensities.at(sensor) = (static_cast(this->counts.at(own * sensors + sensor)) - + static_cast(this->counts.at(other * sensors + sensor))) / + optics.adc_max_counts; + } + + this->config.finish_sequence(); + } +} + +std::vector WallSensors::columns() const { + std::vector names; + names.reserve(this->config.description.sensors.size()); + + for (const WallSensorDescription& sensor : this->config.description.sensors) { + names.push_back(this->config.name + "_" + sensor.name); + } + + return names; +} + +void WallSensors::append(std::vector& row) const { + for (const double intensity : this->intensities) { + row.emplace_back(intensity); + } +} +} // namespace micras::sim diff --git a/engine/src/recording/csv_recorder.cpp b/engine/src/recording/csv_recorder.cpp new file mode 100644 index 0000000..ced1c60 --- /dev/null +++ b/engine/src/recording/csv_recorder.cpp @@ -0,0 +1,89 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/recording/column_source.hpp" +#include "micras/sim/recording/csv_recorder.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/recording/ground_truth.hpp" + +namespace micras::sim { +CsvRecorder::CsvRecorder( + const MujocoWorld& world, GroundTruthConfig config, const std::filesystem::path& path, uint32_t every +) : + ground_truth{world, std::move(config)}, writer{path}, every{std::max(1, every)} { } + +void CsvRecorder::add_source(ColumnSource& source) { + if (this->header_written) { + throw std::logic_error("a column source was added after the CSV header was written"); + } + + this->sources.push_back(&source); +} + +void CsvRecorder::write_header() { + std::vector columns = this->ground_truth.columns(); + + for (ColumnSource* source : this->sources) { + const std::vector names = source->names(); + this->widths.push_back(names.size()); + columns.insert(columns.end(), names.begin(), names.end()); + } + + std::set seen; + + for (const std::string& column : columns) { + if (not seen.insert(column).second) { + throw std::runtime_error("two column sources both write a column named " + column); + } + } + + this->writer.write_header(columns); + this->header_written = true; +} + +void CsvRecorder::on_after_tick(const Simulation& simulation) { + if (simulation.tick() % this->every == 0) { + this->sample(simulation.tick()); + } +} + +void CsvRecorder::sample(uint64_t tick) { + if (not this->header_written) { + if (not std::ranges::all_of(this->sources, [](const ColumnSource* source) { return source->ready(); })) { + return; + } + + this->write_header(); + } + + std::vector cells = this->ground_truth.sample(tick); + + for (std::size_t i = 0; i < this->sources.size(); i++) { + const std::size_t before = cells.size(); + this->sources.at(i)->append(cells); + + if (cells.size() - before != this->widths.at(i)) { + throw std::logic_error( + "a column source appended " + std::to_string(cells.size() - before) + " cells for " + + std::to_string(this->widths.at(i)) + " columns" + ); + } + } + + this->writer.write_row(cells); +} +} // namespace micras::sim diff --git a/src/sim/recording/csv_writer.cpp b/engine/src/recording/csv_writer.cpp similarity index 75% rename from src/sim/recording/csv_writer.cpp rename to engine/src/recording/csv_writer.cpp index 3cdc875..c0ac3d8 100644 --- a/src/sim/recording/csv_writer.cpp +++ b/engine/src/recording/csv_writer.cpp @@ -4,8 +4,17 @@ #include #include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include "micras/sim/core/span_at.hpp" #include "micras/sim/recording/csv_writer.hpp" namespace micras::sim { @@ -17,7 +26,7 @@ CsvWriter::CsvWriter(const std::filesystem::path& path) : file{path} { void CsvWriter::write_header(std::span columns) { for (std::size_t i = 0; i < columns.size(); i++) { - this->file << (i > 0 ? "," : "") << columns[i]; + this->file << (i > 0 ? "," : "") << at(columns, i); } this->file << '\n'; @@ -25,7 +34,7 @@ void CsvWriter::write_header(std::span columns) { void CsvWriter::write_row(std::span cells) { for (std::size_t i = 0; i < cells.size(); i++) { - this->file << (i > 0 ? "," : "") << format(cells[i]); + this->file << (i > 0 ? "," : "") << format(at(cells, i)); } this->file << '\n' << std::flush; @@ -37,7 +46,7 @@ std::string CsvWriter::format(const CsvCell& cell) { if constexpr (std::is_same_v) { std::array buffer{}; const auto result = - std::to_chars(buffer.data(), buffer.data() + buffer.size(), value, std::chars_format::general, 9); + std::to_chars(buffer.data(), std::to_address(buffer.end()), value, std::chars_format::general, 9); if (result.ec != std::errc{}) { throw std::runtime_error("a CSV cell did not fit the formatting buffer"); diff --git a/engine/src/recording/event_log.cpp b/engine/src/recording/event_log.cpp new file mode 100644 index 0000000..03e7141 --- /dev/null +++ b/engine/src/recording/event_log.cpp @@ -0,0 +1,114 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/recording/event_log.hpp" +#include "micras/sim/recording/run_metadata.hpp" + +namespace micras::sim { +/** + * @brief Find the geom a contact pairs with a given one. + * + * @param contact The contact. + * @param geom The geom on one side. + * @return The geom on the other side, or -1 when the contact does not involve the given one. + */ +static int other_geom(const mjContact& contact, int geom) { + if (contact.geom[0] == geom) { + return contact.geom[1]; + } + + if (contact.geom[1] == geom) { + return contact.geom[0]; + } + + return -1; +} + +EventLog::EventLog(std::vector geoms, std::set ignored, StateNames states, const VariableSource* variables) : + geoms{std::move(geoms)}, + ignored{std::move(ignored)}, + states{std::move(states)}, + variables{variables}, + last_touch(this->geoms.size(), -std::numeric_limits::infinity()), + last_values(this->states.size()) { } + +void EventLog::add(RunEvent event) { + if (this->logged.size() < max_events) { + this->logged.push_back(std::move(event)); + } +} + +void EventLog::on_after_tick(const Simulation& simulation) { + const MujocoWorld& world = simulation.context().world; + const mjData* data = world.data(); + + this->log_collisions( + world.model(), std::span(data->contact, static_cast(data->ncon)), data->time + ); + + if (this->variables != nullptr) { + this->log_states(data->time); + } +} + +void EventLog::log_collisions(const mjModel* model, std::span contacts, double now) { + for (std::size_t index = 0; index < this->geoms.size(); index++) { + const int geom = this->geoms.at(index); + const auto touches = std::ranges::any_of(contacts, [this, geom](const mjContact& contact) { + const int other = other_geom(contact, geom); + return other >= 0 and not this->ignored.contains(other); + }); + + if (not touches) { + continue; + } + + if (now - this->last_touch.at(index) > min_separation) { + this->collision_count++; + const char* name = mj_id2name(model, mjOBJ_GEOM, geom); + this->add({.time = now, .kind = "collision", .detail = name == nullptr ? std::to_string(geom) : name}); + } + + this->last_touch.at(index) = now; + } +} + +void EventLog::log_states(double now) { + std::size_t index = 0; + + for (const auto& [variable, names] : this->states) { + const double value = this->variables->value_of(variable); + std::optional& last = this->last_values.at(index++); + + if (std::isnan(value) or last == value) { + continue; + } + + last = value; + const bool named = value >= 0.0 and value < static_cast(names.size()); + std::string name = named ? names.at(static_cast(value)) : std::format("{}", value); + this->add({.time = now, .kind = variable, .detail = std::move(name)}); + } +} +} // namespace micras::sim diff --git a/engine/src/recording/ground_truth.cpp b/engine/src/recording/ground_truth.cpp new file mode 100644 index 0000000..b54392a --- /dev/null +++ b/engine/src/recording/ground_truth.cpp @@ -0,0 +1,235 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/recording/ground_truth.hpp" + +// NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic): mjData exposes flat C arrays indexed by id. + +namespace micras::sim { +/** + * @brief Convert a MuJoCo quaternion to roll, pitch and yaw. + * + * @param quaternion Quaternion as w, x, y, z. + * @return Roll, pitch and yaw in radians, ZYX convention. + */ +static std::array to_euler(const mjtNum* quaternion) { + const double scalar = quaternion[0]; + const double x = quaternion[1]; + const double y = quaternion[2]; + const double z = quaternion[3]; + + const double roll = std::atan2(2.0 * (scalar * x + y * z), 1.0 - 2.0 * (x * x + y * y)); + const double sin_pitch = std::clamp(2.0 * (scalar * y - z * x), -1.0, 1.0); + const double pitch = std::asin(sin_pitch); + const double yaw = std::atan2(2.0 * (scalar * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + + return {roll, pitch, yaw}; +} + +/** + * @brief Find the first velocity dof of a body's free joint. + * + * @param model Loaded model. + * @param body_id Id of the body. + * @param body Name of the body, for the error message. + * @return Index into mjData::qvel. + */ +static int free_joint_dof(const mjModel* model, int body_id, const std::string& body) { + if (model->body_jntnum[body_id] < 1) { + throw std::runtime_error("body '" + body + "' has no joint, a free joint is required"); + } + + const int root_joint = model->body_jntadr[body_id]; + + if (model->jnt_type[root_joint] != mjJNT_FREE) { + throw std::runtime_error( + "the first joint of body '" + body + "' must be a free joint, got mjtJoint " + + std::to_string(model->jnt_type[root_joint]) + ); + } + + return model->jnt_dofadr[root_joint]; +} + +namespace { +/** + * @brief Column names every row starts with, before the robot's own. + */ +const std::array common_columns{ + "tick", "sim_time", "x", "y", "z", "roll", "pitch", + "yaw", "vx_world", "vy_world", "vz_world", "wz_body", "v_forward", +}; +} // namespace + +GroundTruth::GroundTruth(const MujocoWorld& world, GroundTruthConfig config) : + world{world}, + config{std::move(config)}, + body_id{world.require_id(mjOBJ_BODY, this->config.body)}, + free_joint_qvel{free_joint_dof(world.model(), this->body_id, this->config.body)} { + for (const GroundTruthColumn& column : this->config.columns) { + int address = 0; + + switch (column.probe) { + case Probe::JOINT_VELOCITY: + address = world.joint_dof(column.object); + break; + + case Probe::JOINT_POSITION: + address = world.joint_qpos(column.object); + break; + + case Probe::ACTUATOR_FORCE: + address = world.require_id(mjOBJ_ACTUATOR, column.object); + break; + + case Probe::CONTACT_COUNT: + case Probe::CONTACT_NORMAL_FORCE: + case Probe::CONTACT_SLIP: + case Probe::CONTACT_PENETRATION: + address = world.require_id(mjOBJ_GEOM, column.object); + break; + + case Probe::SOLVER_ITERATIONS: + break; + } + + this->resolved.push_back({.probe = column.probe, .address = address}); + } +} + +int64_t GroundTruth::warnings_total(const MujocoWorld& world) { + int64_t warnings = 0; + + for (const mjWarningStat& warning : std::span(world.data()->warning)) { + warnings += warning.number; + } + + return warnings; +} + +std::vector GroundTruth::columns() const { + std::vector names(common_columns.begin(), common_columns.end()); + + for (const GroundTruthColumn& column : this->config.columns) { + names.push_back(column.name); + } + + return names; +} + +GroundTruth::ContactStats GroundTruth::collect_contacts(int geom_id) const { + const mjModel* model = this->world.model(); + const mjData* data = this->world.data(); + ContactStats stats; + + for (int i = 0; i < data->ncon; i++) { + const mjContact& contact = data->contact[i]; + + if (contact.efc_address < 0) { + continue; + } + + if (contact.geom[0] != geom_id and contact.geom[1] != geom_id) { + continue; + } + + stats.count++; + stats.penetration = stats.count == 1 ? contact.dist : std::min(stats.penetration, contact.dist); + + std::array force{}; + mj_contactForce(model, data, i, force.data()); + stats.normal_force += force.at(0); + + if (contact.dim >= 3) { + const int address = contact.efc_address; + stats.slip = std::max(stats.slip, std::hypot(data->efc_vel[address + 1], data->efc_vel[address + 2])); + } + } + + return stats; +} + +const GroundTruth::ContactStats& GroundTruth::contacts_of(int geom_id) { + const auto found = this->row_contacts.find(geom_id); + + if (found != this->row_contacts.end()) { + return found->second; + } + + return this->row_contacts.emplace(geom_id, this->collect_contacts(geom_id)).first->second; +} + +CsvCell GroundTruth::read(const ResolvedColumn& column) { + const mjData* data = this->world.data(); + + switch (column.probe) { + case Probe::JOINT_VELOCITY: + return data->qvel[column.address]; + + case Probe::JOINT_POSITION: + return data->qpos[column.address]; + + case Probe::ACTUATOR_FORCE: + return data->actuator_force[column.address]; + + case Probe::CONTACT_COUNT: + return static_cast(this->contacts_of(column.address).count); + + case Probe::CONTACT_NORMAL_FORCE: + return this->contacts_of(column.address).normal_force; + + case Probe::CONTACT_SLIP: + return this->contacts_of(column.address).slip; + + case Probe::CONTACT_PENETRATION: + return this->contacts_of(column.address).penetration; + + case Probe::SOLVER_ITERATIONS: + return static_cast(data->solver_niter[0]); + } + + throw std::logic_error("unknown ground truth probe"); +} + +std::vector GroundTruth::sample(uint64_t tick) { + const mjData* data = this->world.data(); + const mjtNum* position = data->xpos + 3L * this->body_id; + const auto euler = to_euler(data->xquat + 4L * this->body_id); + const mjtNum* velocity = data->qvel + this->free_joint_qvel; + + const double forward_speed = velocity[0] * std::cos(euler.at(2)) + velocity[1] * std::sin(euler.at(2)); + + std::vector cells{ + tick, data->time, position[0], position[1], position[2], euler.at(0), euler.at(1), + euler.at(2), velocity[0], velocity[1], velocity[2], velocity[5], forward_speed, + }; + + this->row_contacts.clear(); + + for (const ResolvedColumn& column : this->resolved) { + cells.push_back(this->read(column)); + } + + return cells; +} +} // namespace micras::sim + +// NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) diff --git a/src/sim/recording/run_metadata.cpp b/engine/src/recording/run_metadata.cpp similarity index 67% rename from src/sim/recording/run_metadata.cpp rename to engine/src/recording/run_metadata.cpp index f75475a..42d8c6e 100644 --- a/src/sim/recording/run_metadata.cpp +++ b/engine/src/recording/run_metadata.cpp @@ -2,31 +2,37 @@ * @file */ -#include +#include +#include #include #include +#include +#include #include #include #include #include +#include #include +#include "micras/sim/core/span_at.hpp" #include "micras/sim/recording/run_metadata.hpp" namespace micras::sim { -namespace { /** * @brief Escape a string as a JSON string literal body. * * @param value Raw string. * @return Escaped string, without the surrounding quotes. */ -std::string json_escape(const std::string& value) { +static std::string json_escape(const std::string& value) { std::string escaped; escaped.reserve(value.size()); - for (const unsigned char character : value) { + for (const char raw : value) { + const auto character = static_cast(raw); + switch (character) { case '"': escaped += "\\\""; @@ -63,7 +69,7 @@ std::string json_escape(const std::string& value) { * @param value Value of the field. * @param last Whether this is the last field of the object. */ -void field(std::ostringstream& out, const char* name, const std::string& value, bool last = false) { +static void field(std::ostringstream& out, const char* name, const std::string& value, bool last = false) { out << " \"" << name << "\": \"" << json_escape(value) << '"' << (last ? "\n" : ",\n"); } @@ -75,7 +81,7 @@ void field(std::ostringstream& out, const char* name, const std::string& value, * @param value Value of the field. * @param last Whether this is the last field of the object. */ -void field(std::ostringstream& out, const char* name, bool value, bool last = false) { +static void field(std::ostringstream& out, const char* name, bool value, bool last = false) { out << " \"" << name << "\": " << (value ? "true" : "false") << (last ? "\n" : ",\n"); } @@ -92,36 +98,56 @@ void field(std::ostringstream& out, const char* name, bool value, bool last = fa */ template requires std::is_arithmetic_v -void field(std::ostringstream& out, const char* name, T value, bool last = false) { +static void field(std::ostringstream& out, const char* name, T value, bool last = false) { out << " \"" << name << "\": " << value << (last ? "\n" : ",\n"); } -} // namespace std::string RunMetadata::to_json() const { std::ostringstream out; out.imbue(std::locale::classic()); out << "{\n"; + field(out, "target", this->target); + field(out, "target_dir", this->target_dir); field(out, "firmware_sha", this->firmware_sha); - field(out, "model_path", this->model_path); - field(out, "model_sha256", this->model_sha256); + field(out, "robot_path", this->robot_path); + field(out, "robot_sha256", this->robot_sha256); + field(out, "scenario_path", this->scenario_path); field(out, "maze_path", this->maze_path); + field(out, "model_sha256", this->model_sha256); field(out, "mujoco_version", this->mujoco_version); field(out, "compiler", this->compiler); field(out, "build_type", this->build_type); field(out, "args", this->args); + field(out, "seed", this->seed); + field(out, "ideal", this->ideal); field(out, "loop_time_us", this->loop_time_us); field(out, "timestep", this->timestep); field(out, "steps_per_tick", this->steps_per_tick); + field(out, "record_every", this->record_every); field(out, "requested_ticks", this->requested_ticks); field(out, "ticks", this->ticks); field(out, "sim_time", this->sim_time); + field(out, "stopped_at", this->stopped_at); field(out, "final_z", this->final_z); - field(out, "pool_columns", this->pool_columns); - field(out, "telemetry_resyncs", this->telemetry_resyncs); + + for (const MetadataField& target_field : this->target_fields) { + field(out, target_field.name.c_str(), target_field.value); + } + field(out, "warnings_total", this->warnings_total); - field(out, "interactive", this->interactive, true); - out << "}\n"; + field(out, "serial_dropped_bytes", this->serial_dropped_bytes); + field(out, "bridge_dropped_frames", this->bridge_dropped_frames); + field(out, "interactive", this->interactive); + out << " \"events\": ["; + + for (std::size_t i = 0; i < this->events.size(); i++) { + const RunEvent& event = this->events.at(i); + out << (i > 0 ? "," : "") << "\n {" << R"("time": )" << event.time << R"(, "kind": ")" + << json_escape(event.kind) << R"(", "detail": ")" << json_escape(event.detail) << R"("})"; + } + + out << (this->events.empty() ? "]\n" : "\n ]\n") << "}\n"; return out.str(); } @@ -135,6 +161,10 @@ void RunMetadata::write(const std::filesystem::path& path) const { file << this->to_json(); } +std::string RunMetadata::sha256_of_text(const std::string& text) { + return picosha2::hash256_hex_string(text); +} + std::string RunMetadata::sha256_of(const std::filesystem::path& path) { std::ifstream file(path, std::ios::binary); @@ -150,7 +180,7 @@ std::string RunMetadata::join_args(std::span arguments) { for (std::size_t i = 1; i < arguments.size(); i++) { joined += (i > 1 ? " " : ""); - joined += arguments[i]; + joined += at(arguments, i); } return joined; diff --git a/engine/src/robot/robot_description.cpp b/engine/src/robot/robot_description.cpp new file mode 100644 index 0000000..9135bd9 --- /dev/null +++ b/engine/src/robot/robot_description.cpp @@ -0,0 +1,502 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/text_file.hpp" +#include "micras/sim/robot/robot_description.hpp" + +namespace micras::sim { +namespace { +/** + * @brief Radians per degree. + */ +constexpr double radians_per_degree{std::numbers::pi / 180.0}; + +/** + * @brief One table of the file, read key by key, which refuses keys nobody read. + */ +class Section { +public: + /** + * @brief Wrap a table. + * + * @param table Table to read. + * @param path Dotted path of the table, for messages. + */ + Section(const toml::table& table, std::string path) : table{&table}, path{std::move(path)} { } + + /** + * @brief Read a number. + * + * @param key Key in this table. + * @return The number. + */ + double number(std::string_view key) { + const toml::node& node = this->value(key); + + if (const auto number = node.value(); number.has_value()) { + return *number; + } + + throw this->error(key, "a number"); + } + + /** + * @brief Read an angle given in degrees. + * + * @param key Key in this table, which should end in _deg. + * @return The angle in radians. + */ + double degrees(std::string_view key) { return this->number(key) * radians_per_degree; } + + /** + * @brief Read a whole number. + * + * @param key Key in this table. + * @return The number. + */ + int64_t integer(std::string_view key) { + const toml::node& node = this->value(key); + + if (const auto number = node.value(); number.has_value() and node.is_integer()) { + return *number; + } + + throw this->error(key, "a whole number"); + } + + /** + * @brief Read a text. + * + * @param key Key in this table. + * @return The text. + */ + std::string text(std::string_view key) { + const toml::node& node = this->value(key); + + if (const auto text = node.value(); text.has_value()) { + return *text; + } + + throw this->error(key, "a text"); + } + + /** + * @brief Read three numbers. + * + * @param key Key in this table. + * @return The numbers. + */ + Vector3 vector3(std::string_view key) { + const std::vector numbers = this->numbers(key, 3); + return {numbers.at(0), numbers.at(1), numbers.at(2)}; + } + + /** + * @brief Read a list of points in the plane. + * + * @param key Key in this table. + * @return The points. + */ + std::vector> points(std::string_view key) { + const toml::array* array = this->value(key).as_array(); + + if (array == nullptr or array->empty()) { + throw this->error(key, "a list of [x, y] points"); + } + + std::vector> points; + + for (const toml::node& point : *array) { + const toml::array* pair = point.as_array(); + + if (pair == nullptr or pair->size() != 2 or not pair->at(0).is_number() or not pair->at(1).is_number()) { + throw this->error(key, "a list of [x, y] points"); + } + + const std::optional x = pair->at(0).value(); + const std::optional y = pair->at(1).value(); + + if (not x.has_value() or not y.has_value()) { + throw this->error(key, "a list of [x, y] points"); + } + + points.push_back({*x, *y}); + } + + return points; + } + + /** + * @brief Open a sub-table. + * + * @param key Key in this table. + * @return The sub-table. + */ + Section section(std::string_view key) { + this->used.insert(std::string{key}); + const toml::table* table = this->table->get_as(key); + + if (table == nullptr) { + throw std::runtime_error(std::format("{}: [{}] is missing", this->where(), this->child(key))); + } + + return {*table, this->child(key)}; + } + + /** + * @brief Open every table of an array of tables. + * + * @param key Key in this table. + * @return The tables, possibly none. + */ + std::vector
sections(std::string_view key) { + this->used.insert(std::string{key}); + std::vector
sections; + const toml::array* const array = this->table->get_as(key); + + if (array == nullptr) { + return sections; + } + + for (std::size_t i = 0; i < array->size(); i++) { + const toml::table* table = array->at(i).as_table(); + + if (table == nullptr) { + throw std::runtime_error(std::format("{}: {} must be an array of tables", this->where(), key)); + } + + sections.emplace_back(*table, std::format("{}[{}]", this->child(key), i)); + } + + return sections; + } + + /** + * @brief Refuse any key that was not read. + */ + void finish() const { + for (const auto& [key, node] : *this->table) { + if (not this->used.contains(std::string{key.str()})) { + throw std::runtime_error(std::format("{}: unknown key {}", this->where(), this->child(key.str()))); + } + } + } + +private: + /** + * @brief Find a value, unwrapping { value, source }. + * + * @param key Key in this table. + * @return The value node. + */ + const toml::node& value(std::string_view key) { + this->used.insert(std::string{key}); + const toml::node* node = this->table->get(key); + + if (node == nullptr) { + throw std::runtime_error(std::format("{}: {} is missing", this->where(), this->child(key))); + } + + if (const toml::table* sourced = node->as_table(); sourced != nullptr) { + const toml::node* value = sourced->get("value"); + const auto* const source = sourced->get_as("source"); + + if (value == nullptr or source == nullptr or sourced->size() != 2) { + throw this->error(key, "a value or a { value = ..., source = \"...\" } table"); + } + + return *value; + } + + return *node; + } + + /** + * @brief Read a fixed number of numbers. + * + * @param key Key in this table. + * @param count Number of numbers. + * @return The numbers. + */ + std::vector numbers(std::string_view key, std::size_t count) { + const toml::array* array = this->value(key).as_array(); + + if (array == nullptr or array->size() != count) { + throw this->error(key, std::format("{} numbers", count)); + } + + std::vector numbers; + + for (const toml::node& element : *array) { + const std::optional number = element.value(); + + if (not element.is_number() or not number.has_value()) { + throw this->error(key, std::format("{} numbers", count)); + } + + numbers.push_back(*number); + } + + return numbers; + } + + /** + * @brief Build an error for a value of the wrong kind. + * + * @param key Key in this table. + * @param expected What was expected. + * @return The error. + */ + std::runtime_error error(std::string_view key, std::string_view expected) const { + return std::runtime_error(std::format("{}: {} must be {}", this->where(), this->child(key), expected)); + } + + /** + * @brief Get the dotted path of a key of this table. + * + * @param key Key in this table. + * @return The path. + */ + std::string child(std::string_view key) const { + return this->path.empty() ? std::string{key} : this->path + "." + std::string{key}; + } + + /** + * @brief Name the file, for messages. + * + * @return The origin of the table. + */ + std::string where() const { + const auto& source = this->table->source(); + return source.path == nullptr ? std::string{"robot.toml"} : std::string{*source.path}; + } + + const toml::table* table; + std::string path; + std::set used; +}; +} // namespace + +/** + * @brief Read the chassis table. + * + * @param section The [chassis] table. + * @return The chassis. + */ +static ChassisDescription read_chassis(Section section) { + ChassisDescription chassis{ + .mass = section.number("mass"), + .center_of_mass = section.vector3("center_of_mass"), + .inertia = section.vector3("inertia"), + .outline = section.points("outline"), + .board_bottom = section.number("board_bottom"), + .board_top = section.number("board_top"), + .friction = section.number("friction"), + .boxes = {}, + .cylinders = {}, + .skids = {}, + }; + + for (Section part : section.sections("boxes")) { + chassis.boxes.push_back({.name = part.text("name"), .min = part.vector3("min"), .max = part.vector3("max")}); + part.finish(); + } + + for (Section part : section.sections("skids")) { + chassis.skids.push_back( + {.name = part.text("name"), + .center = part.vector3("center"), + .radius = part.number("radius"), + .friction = part.number("friction")} + ); + part.finish(); + } + + for (Section part : section.sections("cylinders")) { + chassis.cylinders.push_back( + {.name = part.text("name"), + .base = part.vector3("base"), + .radius = part.number("radius"), + .height = part.number("height")} + ); + part.finish(); + } + + section.finish(); + return chassis; +} + +/** + * @brief Read the wall sensors table. + * + * @param section The [wall_sensors] table. + * @return The wall sensors. + */ +static WallSensorsDescription read_wall_sensors(Section section) { + WallSensorsDescription sensors{ + .emitter_half_angle = section.degrees("emitter_half_angle_deg"), + .emitter_intensity = section.number("emitter_intensity"), + .receiver_half_angle = section.degrees("receiver_half_angle_deg"), + .receiver_responsivity = section.number("receiver_responsivity"), + .load_resistance = section.number("load_resistance"), + .saturation_voltage = section.number("saturation_voltage"), + .adc_reference = section.number("adc_reference"), + .adc_max_counts = section.number("adc_max_counts"), + .adc_noise_counts = section.number("adc_noise_counts"), + .settle_fraction = section.number("settle_fraction"), + .ambient_irradiance = section.number("ambient_irradiance"), + .rays = static_cast(section.integer("rays")), + .sensors = {}, + }; + + for (Section sensor : section.sections("sensors")) { + sensors.sensors.push_back( + {.name = sensor.text("name"), + .emitter = sensor.vector3("emitter"), + .receiver = sensor.vector3("receiver"), + .yaw = sensor.degrees("yaw_deg"), + .group = static_cast(sensor.integer("group")), + .gain = sensor.number("gain")} + ); + sensor.finish(); + } + + section.finish(); + return sensors; +} + +/** + * @brief Read everything but the chassis and the wall sensors. + * + * @param root The whole file. + * @param robot Description to fill. + */ +static void read_components(Section& root, RobotDescription& robot) { + Section integration = root.section("integration"); + robot.integration = {.timestep = integration.number("timestep")}; + integration.finish(); + + Section wheels = root.section("wheels"); + robot.wheels = { + .radius = wheels.number("radius"), + .width = wheels.number("width"), + .track = wheels.number("track"), + .mass = wheels.number("mass"), + .spin_inertia = wheels.number("spin_inertia"), + .friction = wheels.number("friction"), + .torsional_friction = wheels.number("torsional_friction"), + .contact_time_constant = wheels.number("contact_time_constant"), + .contact_damping_ratio = wheels.number("contact_damping_ratio"), + }; + wheels.finish(); + + Section drive = root.section("drive"); + robot.drive = { + .winding_resistance = drive.number("winding_resistance"), + .bridge_resistance = drive.number("bridge_resistance"), + .torque_constant = drive.number("torque_constant"), + .speed_constant = drive.number("speed_constant"), + .no_load_current = drive.number("no_load_current"), + .rotor_inertia = drive.number("rotor_inertia"), + .gear_ratio = drive.number("gear_ratio"), + .gear_efficiency = drive.number("gear_efficiency"), + .supply_voltage = drive.number("supply_voltage"), + }; + drive.finish(); + + Section encoders = root.section("encoders"); + robot.encoders = {.counts_per_revolution = static_cast(encoders.integer("counts_per_revolution"))}; + encoders.finish(); + + Section imu = root.section("imu"); + robot.imu = { + .position = imu.vector3("position"), + .axes = {imu.vector3("chip_x"), imu.vector3("chip_y")}, + .gyro_noise_density = imu.number("gyro_noise_density"), + .gyro_bias = imu.number("gyro_bias"), + .gyro_scale_error = imu.number("gyro_scale_error"), + .gyro_resolution = imu.number("gyro_resolution"), + .accel_noise_density = imu.number("accel_noise_density"), + .accel_bias = imu.number("accel_bias"), + .accel_resolution = imu.number("accel_resolution"), + .bandwidth = imu.number("bandwidth"), + .output_rate = imu.number("output_rate"), + .clock_error = imu.number("clock_error"), + }; + imu.finish(); + + Section fan = root.section("fan"); + robot.fan = { + .position = fan.vector3("position"), + .max_downforce = fan.number("max_downforce"), + .nominal_voltage = fan.number("nominal_voltage"), + .time_constant = fan.number("time_constant"), + }; + fan.finish(); + + Section battery = root.section("battery"); + robot.battery = { + .cells = static_cast(battery.integer("cells")), + .cell_voltage = battery.number("cell_voltage"), + }; + battery.finish(); + + Section link = root.section("link"); + robot.link = {.baud_rate = static_cast(link.integer("baud_rate"))}; + link.finish(); +} + +RobotDescription RobotDescription::load(const std::filesystem::path& path) { + return parse(read_text_file(path, "robot description"), path.string()); +} + +RobotDescription RobotDescription::parse(std::string_view text, const std::string& origin) { + toml::table table; + + try { + table = toml::parse(text, origin); + } catch (const toml::parse_error& error) { + throw std::runtime_error(std::format("{}: {}", origin, error.description())); + } + + RobotDescription robot; + Section root{table, ""}; + + const int64_t schema = root.integer("schema"); + + if (schema != schema_version) { + throw std::runtime_error( + std::format("{}: schema {} is not the {} this build reads", origin, schema, schema_version) + ); + } + + robot.name = root.text("name"); + read_components(root, robot); + robot.chassis = read_chassis(root.section("chassis")); + robot.wall_sensors = read_wall_sensors(root.section("wall_sensors")); + root.finish(); + + if (robot.chassis.outline.size() < 3) { + throw std::runtime_error(origin + ": chassis.outline needs at least three points"); + } + + return robot; +} +} // namespace micras::sim diff --git a/engine/src/robot/robot_model.cpp b/engine/src/robot/robot_model.cpp new file mode 100644 index 0000000..9fe0f96 --- /dev/null +++ b/engine/src/robot/robot_model.cpp @@ -0,0 +1,250 @@ +/** + * @file + */ + +#include +#include +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/robot/robot_model.hpp" + +namespace micras::sim { +/** + * @brief Format a vector for an MJCF attribute. + * + * @param vector The vector. + * @return Space separated components. + */ +static std::string text(const Vector3& vector) { + return std::format("{:.9g} {:.9g} {:.9g}", vector.at(0), vector.at(1), vector.at(2)); +} + +/** + * @brief Generate the geoms of the chassis. + * + * @param robot The description. + * @param names Names of the model. + * @return MJCF of the geoms. + */ +static std::string chassis_geoms(const RobotDescription& robot, const RobotModelNames& names) { + const ChassisDescription& chassis = robot.chassis; + std::string geoms = std::format( + " \n", + names.board, robot.name, robot.name, chassis.friction + ); + + for (const BoxPart& part : chassis.boxes) { + const Vector3 center{ + (part.min.at(0) + part.max.at(0)) / 2, (part.min.at(1) + part.max.at(1)) / 2, + (part.min.at(2) + part.max.at(2)) / 2 + }; + const Vector3 half{ + std::abs(part.max.at(0) - part.min.at(0)) / 2, std::abs(part.max.at(1) - part.min.at(1)) / 2, + std::abs(part.max.at(2) - part.min.at(2)) / 2 + }; + + geoms += std::format( + " \n", part.name, + text(center), text(half), robot.name + ); + } + + for (const CylinderPart& part : chassis.cylinders) { + const Vector3 center{part.base.at(0), part.base.at(1), part.base.at(2) + part.height / 2}; + + geoms += std::format( + " \n", + part.name, text(center), part.radius, part.height / 2, robot.name + ); + } + + for (const SkidPart& part : chassis.skids) { + geoms += std::format( + " \n", + part.name, text(part.center), part.radius, robot.name + ); + } + + return geoms; +} + +/** + * @brief Generate one wheel body. + * + * @param robot The description. + * @param name Name of the wheel's body, joint and geom. + * @param side +1 for the left wheel, -1 for the right one. + * @return MJCF of the wheel. + * + * @note The tire has sliding and torsional friction but no rolling friction + * (condim 4, not 6). MuJoCo's convex contact separates two surfaces in + * proportion to how fast their friction is slipping, and a rolling wheel + * keeps a rolling-friction constraint slipping all the time, so the tire + * would hop. The same separation during a pivot, where the tires really + * do scrub, is absorbed by a soft enough tire, which is part of why + * contact_time_constant is what it is. + */ +static std::string wheel_body(const RobotDescription& robot, const std::string& name, double side) { + const WheelsDescription& wheels = robot.wheels; + const DriveDescription& drive = robot.drive; + const double armature = drive.rotor_inertia * drive.gear_ratio * drive.gear_ratio; + const double friction_torque = drive.torque_constant * drive.no_load_current * drive.gear_ratio; + const double transverse = wheels.mass * (3 * wheels.radius * wheels.radius + wheels.width * wheels.width) / 12; + + return std::format( + " \n" + " \n" + " \n" + " \n" + " \n", + name, side * wheels.track / 2, wheels.radius, wheels.mass, transverse, wheels.spin_inertia, armature, + friction_torque, wheels.width / 2, robot.name, wheels.friction, wheels.torsional_friction, + wheels.contact_time_constant, wheels.contact_damping_ratio + ); +} + +/** + * @brief Generate the sites the devices sample at. + * + * @param robot The description. + * @param names Names of the model. + * @return MJCF of the sites. + */ +static std::string sites(const RobotDescription& robot, const RobotModelNames& names) { + const ImuDescription& imu = robot.imu; + std::string sites = std::format( + " \n" + " \n", + names.imu, text(imu.position), text(imu.axes.at(0)), text(imu.axes.at(1)), names.fan, text(robot.fan.position) + ); + + for (const WallSensorDescription& sensor : robot.wall_sensors.sensors) { + sites += std::format( + " \n" + " \n", + sensor.name, text(sensor.emitter), text(sensor.receiver), sensor.yaw + ); + } + + return sites; +} + +/** + * @brief Generate the board outline as a mesh, extruded between the board's faces. + * + * @param robot The description. + * @return Space separated vertices. + */ +static std::string board_vertices(const RobotDescription& robot) { + std::string vertices; + + for (const auto& point : robot.chassis.outline) { + vertices += std::format( + "{:.9g} {:.9g} {:.9g} {:.9g} {:.9g} {:.9g} ", point.at(0), point.at(1), robot.chassis.board_bottom, + point.at(0), point.at(1), robot.chassis.board_top + ); + } + + return vertices; +} + +namespace { +/** + * @brief Height of the onboard camera above the top of the board, in meters. + * + * @note Below the top of the walls of a maze, so that the view is the corridor the robot is in. + */ +constexpr double onboard_height{0.025}; + +/** + * @brief Distance of the onboard camera ahead of the outline of the board, in meters. + * + * @note The sensor housings stand a little past the board, and would fill the bottom of the view. + */ +constexpr double onboard_lead{0.01}; +} // namespace + +/** + * @brief Get where the onboard camera sits ahead of the body origin. + * + * @param robot The description. + * @return The largest forward coordinate of the outline, plus the lead of the camera. + */ +static double front_of(const RobotDescription& robot) { + double front = 0.0; + + for (const auto& point : robot.chassis.outline) { + front = std::max(front, static_cast(point.at(0))); + } + + return front + onboard_lead; +} + +std::string robot_mjcf(const RobotDescription& robot) { + const RobotModelNames names = RobotModelNames::of(robot); + const DriveDescription& drive = robot.drive; + const double resistance = drive.resistance(); + const double gain = drive.gear_ratio * drive.gear_efficiency * drive.torque_constant / resistance; + const double damping = + drive.gear_ratio * drive.gear_ratio * drive.torque_constant * drive.speed_constant / resistance; + const ChassisDescription& chassis = robot.chassis; + + return std::format( + "\n" + " \n" + " \n", + robot.name, robot.integration.timestep, MujocoWorld::unseen_group, board_vertices(robot), names.body, + text(chassis.center_of_mass), chassis.mass, text(chassis.inertia), chassis_geoms(robot, names), + sites(robot, names), wheel_body(robot, names.left_wheel, 1.0), wheel_body(robot, names.right_wheel, -1.0), + names.left_motor, names.right_motor, names.left_wheel, names.right_wheel, gain, -damping, + -2 * drive.supply_voltage, 2 * drive.supply_voltage, names.fan, 4 * robot.fan.max_downforce, names.gyro, + names.accelerometer, names.imu, names.fan_reference, front_of(robot), chassis.board_top + onboard_height + ); +} +} // namespace micras::sim diff --git a/engine/src/scenario/scenario.cpp b/engine/src/scenario/scenario.cpp new file mode 100644 index 0000000..3b07e0f --- /dev/null +++ b/engine/src/scenario/scenario.cpp @@ -0,0 +1,434 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/text_file.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/scenario/scenario.hpp" + +namespace micras::sim { +/** + * @brief Refuse keys of a table outside a known set. + * + * @param table Table to check. + * @param known Keys it may have. + * @param where Name of the table, for messages. + */ +static void + refuse_unknown(const toml::table& table, const std::set& known, const std::string& where) { + for (const auto& [key, node] : table) { + if (not known.contains(key.str())) { + throw std::runtime_error(std::format("{}: unknown key {}", where, key.str())); + } + } +} + +/** + * @brief Read a required number. + * + * @param table Table to read. + * @param key Key. + * @param where Name of the table, for messages. + * @return The number. + */ +static double require_number(const toml::table& table, std::string_view key, const std::string& where) { + const std::optional value = table[key].value(); + + if (not value.has_value()) { + throw std::runtime_error(std::format("{}: {} must be a number", where, key)); + } + + return *value; +} + +/** + * @brief Format a stop condition's value as text, a number or a name. + * + * @param node The value's node. + * @param where Name of the table, for messages. + * @return The value as text. + */ +static std::string condition_value(const toml::node_view& node, const std::string& where) { + if (const auto text = node.value(); text.has_value()) { + return *text; + } + + if (const auto number = node.value(); number.has_value()) { + return std::format("{}", *number); + } + + throw std::runtime_error(where + ": equals must be a number or a name"); +} + +/** + * @brief Read a condition's values, given as one value or as an array of them. + * + * @param node The values' node; a missing one gives no value. + * @param where Name of the table, for messages. + * @return The values as text. + */ +static std::vector + condition_values(const toml::node_view& node, const std::string& where) { + std::vector values; + + if (const toml::array* array = node.as_array(); array != nullptr) { + for (const toml::node& value : *array) { + values.push_back(condition_value(toml::node_view{value}, where)); + } + } else if (node) { + values.push_back(condition_value(node, where)); + } + + return values; +} + +/** + * @brief Read the stop condition. + * + * @param table The [stop] table. + * @param where Name of the table, for messages. + * @return The condition. + */ +static StopCondition read_stop(const toml::table& table, const std::string& where) { + refuse_unknown(table, {"when", "equals", "after", "count", "abort"}, where); + const std::optional variable = table["when"].value(); + + if (not variable.has_value()) { + throw std::runtime_error(where + ": when must name a firmware variable"); + } + + StopCondition stop{ + .variable = *variable, + .equals = condition_values(table["equals"], where), + .after = table["after"].value_or(0.0), + .count = static_cast(table["count"].value_or(int64_t{1})), + .abort = condition_values(table["abort"], where), + }; + + if (stop.equals.empty()) { + throw std::runtime_error(where + ": equals names no value"); + } + + if (stop.count < 1) { + throw std::runtime_error(where + ": count must be at least 1"); + } + + return stop; +} + +/** + * @brief Read one event of the timeline. + * + * @param table The event's table. + * @param where Name of the event, for messages. + * @return The event. + */ +static ScenarioEvent read_event(const toml::table& table, const std::string& where) { + refuse_unknown(table, {"at", "press", "for", "set", "value", "send", "when", "equals"}, where); + + ScenarioEvent event; + event.at = require_number(table, "at", where); + const int kinds = static_cast(table.contains("press")) + static_cast(table.contains("set")) + + static_cast(table.contains("send")); + + if (kinds != 1) { + throw std::runtime_error(where + ": an event is exactly one of press, set or send"); + } + + if (const auto press = table["press"].value(); press.has_value()) { + event.kind = ScenarioEvent::Kind::PRESS; + event.target = *press; + event.duration = require_number(table, "for", where); + } else if (const auto set = table["set"].value(); set.has_value()) { + event.kind = ScenarioEvent::Kind::SET; + event.target = *set; + const std::optional value = table["value"].value(); + + if (not value.has_value()) { + throw std::runtime_error(where + ": set needs a true or false value"); + } + + event.value = *value; + } else { + const std::optional send = table["send"].value(); + + if (not send.has_value()) { + throw std::runtime_error(where + ": send must name a message"); + } + + event.kind = ScenarioEvent::Kind::SEND; + event.target = *send; + } + + if (event.at < 0.0 or event.duration < 0.0) { + throw std::runtime_error(where + ": times cannot be negative"); + } + + if (table.contains("when") != table.contains("equals")) { + throw std::runtime_error(where + ": a condition needs both when and equals"); + } + + if (table.contains("when")) { + const std::optional variable = table["when"].value(); + + if (not variable.has_value()) { + throw std::runtime_error(where + ": when must name a firmware variable"); + } + + event.when = *variable; + event.equals = condition_value(table["equals"], where); + } + + return event; +} + +Scenario Scenario::load(const std::filesystem::path& path) { + return parse(read_text_file(path, "scenario"), path.string()); +} + +Scenario Scenario::parse(std::string_view text, const std::string& origin) { + toml::table table; + + try { + table = toml::parse(text, origin); + } catch (const toml::parse_error& error) { + throw std::runtime_error(std::format("{}: {}", origin, error.description())); + } + + refuse_unknown(table, {"robot", "arena", "seconds", "seed", "start", "events", "stop"}, origin); + + Scenario scenario; + scenario.robot = table["robot"].value_or(std::string{}); + scenario.arena = table["arena"].value_or(std::string{}); + scenario.seconds = table["seconds"].value_or(0.0); + + if (const auto seed = table["seed"].value(); seed.has_value()) { + scenario.seed = static_cast(*seed); + } + + if (const toml::table* start = table["start"].as_table(); start != nullptr) { + const std::string where = origin + ": [start]"; + refuse_unknown(*start, {"x", "y", "yaw_deg"}, where); + scenario.start = MujocoWorld::Placement{ + .x = require_number(*start, "x", where), + .y = require_number(*start, "y", where), + .yaw = require_number(*start, "yaw_deg", where) * std::numbers::pi / 180.0, + }; + } + + if (const toml::array* events = table["events"].as_array(); events != nullptr) { + for (std::size_t index = 0; index < events->size(); index++) { + const toml::table* event = events->at(index).as_table(); + const std::string where = std::format("{}: events[{}]", origin, index); + + if (event == nullptr) { + throw std::runtime_error(where + " must be a table"); + } + + scenario.events.push_back(read_event(*event, where)); + } + } + + std::ranges::stable_sort(scenario.events, {}, &ScenarioEvent::at); + + if (const toml::table* stop = table["stop"].as_table(); stop != nullptr) { + scenario.stop = read_stop(*stop, origin + ": [stop]"); + } + + return scenario; +} + +ScenarioPlayer::ScenarioPlayer( + Scenario scenario, const ScenarioHooks& hooks, SerialBus& serial, const VariableSource* variables +) : + scenario{std::move(scenario)}, hooks{hooks}, serial{serial}, variables{variables} { + for (const ScenarioEvent& event : this->scenario.events) { + this->check(event); + this->event_values.push_back( + event.when.empty() ? std::numeric_limits::quiet_NaN() : this->resolve(event.when, event.equals) + ); + } + + if (not this->scenario.stop.has_value()) { + return; + } + + const StopCondition& stop = *this->scenario.stop; + + for (const std::string& value : stop.equals) { + this->stop_values.push_back(this->resolve(stop.variable, value)); + } + + for (const std::string& value : stop.abort) { + this->abort_values.push_back(this->resolve(stop.variable, value)); + } +} + +double ScenarioPlayer::resolve(const std::string& variable, const std::string& value) const { + double number{}; + const char* last = std::to_address(value.cend()); + + if (std::from_chars(value.data(), last, number).ptr == last) { + return number; + } + + const auto names = this->hooks.state_names.find(variable); + + if (names == this->hooks.state_names.end()) { + throw std::runtime_error("the scenario names state " + value + ", but " + variable + " has no state names"); + } + + const auto found = std::ranges::find(names->second, value); + + if (found == names->second.end()) { + throw std::runtime_error(variable + " has no state named " + value); + } + + return static_cast(std::distance(names->second.begin(), found)); +} + +bool ScenarioPlayer::is_due(std::size_t index) const { + const ScenarioEvent& event = this->scenario.events.at(index); + + if (event.when.empty()) { + return true; + } + + return this->variables != nullptr and this->variables->value_of(event.when) == this->event_values.at(index); +} + +void ScenarioPlayer::check(const ScenarioEvent& event) const { + switch (event.kind) { + case ScenarioEvent::Kind::PRESS: + case ScenarioEvent::Kind::SET: + if (not this->hooks.inputs.contains(event.target)) { + throw std::runtime_error("the robot has no input named " + event.target); + } + + break; + + case ScenarioEvent::Kind::SEND: + if (not this->hooks.messages.contains(event.target)) { + throw std::runtime_error("the robot has no message named " + event.target); + } + + break; + } +} + +RunControl ScenarioPlayer::on_before_tick(const Simulation& simulation) { + const Clock& clock = simulation.context().clock; + const uint64_t tick = simulation.tick(); + const double now = static_cast(tick) * clock.us_per_tick() * 1e-6; + + for (auto release = this->releases.begin(); release != this->releases.end();) { + if (tick >= release->tick) { + if (not this->human) { + release->input->set(false); + } + + release = this->releases.erase(release); + } else { + ++release; + } + } + + while (this->next_event < this->scenario.events.size() and + clock.tick_at(this->scenario.events.at(this->next_event).at) <= tick and this->is_due(this->next_event)) { + const ScenarioEvent& event = this->scenario.events.at(this->next_event++); + const uint64_t end = tick + std::max(1, clock.tick_at(event.duration)); + + switch (event.kind) { + case ScenarioEvent::Kind::PRESS: + if (not this->human) { + DigitalInput* input = this->hooks.inputs.find(event.target)->second; + input->set(true); + this->releases.push_back({.input = input, .tick = end}); + } + + break; + + case ScenarioEvent::Kind::SET: + if (not this->human) { + this->hooks.inputs.find(event.target)->second->set(event.value); + } + + break; + + case ScenarioEvent::Kind::SEND: + this->serial.queue_for_firmware(this->hooks.messages.find(event.target)->second); + break; + } + } + + return this->check_stop(now); +} + +RunControl ScenarioPlayer::check_stop(double now) { + if (not this->scenario.stop.has_value() or this->variables == nullptr or this->stop_time.has_value()) { + return this->stop_time.has_value() ? RunControl::QUIT : RunControl::RUN; + } + + const StopCondition& stop = *this->scenario.stop; + + if (now < stop.after) { + return RunControl::RUN; + } + + const double current = this->variables->value_of(stop.variable); + const auto found = std::ranges::find(this->stop_values, current); + const bool matching = found != this->stop_values.end(); + + if (matching and not this->stop_matching) { + this->stop_reached++; + } + + this->stop_matching = matching; + + if (matching and this->stop_reached >= stop.count) { + this->stop_time = now; + const auto index = static_cast(std::distance(this->stop_values.begin(), found)); + std::cout << "stop condition " << stop.variable << " == " << stop.equals.at(index) << " held at " << now + << " s\n"; + return RunControl::QUIT; + } + + const auto aborted = std::ranges::find(this->abort_values, current); + + if (aborted != this->abort_values.end()) { + this->stop_time = now; + const auto index = static_cast(std::distance(this->abort_values.begin(), aborted)); + std::cout << "stop condition " << stop.variable << " == " << stop.abort.at(index) << " (abort) held at " << now + << " s\n"; + return RunControl::QUIT; + } + + return RunControl::RUN; +} +} // namespace micras::sim diff --git a/include/micras/proxy/argb.hpp b/include/micras/proxy/argb.hpp deleted file mode 100644 index c828b80..0000000 --- a/include/micras/proxy/argb.hpp +++ /dev/null @@ -1,146 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_ARGB_HPP -#define MICRAS_PROXY_ARGB_HPP - -#include -#include -#include - -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Stub addressable RGB LED strip that only records its last colors. - */ -template -class TArgb { -public: - /** - * @brief Struct for storing color information. - */ - struct Color { - uint8_t red; - uint8_t green; - uint8_t blue; - - Color operator*(float brightness) const { - return { - static_cast(this->red * brightness), - static_cast(this->green * brightness), - static_cast(this->blue * brightness), - }; - } - }; - - /** - * @brief Configuration struct for the addressable RGB LED. - */ - struct Config { - std::array names; - - /** - * @brief Simulation holding the state this strip is shown in. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Predefined colors. - */ - struct Colors { - Colors() = delete; - - static constexpr Color red{255, 0, 0}; - static constexpr Color green{0, 255, 0}; - static constexpr Color blue{0, 0, 255}; - static constexpr Color yellow{255, 255, 0}; - static constexpr Color cyan{0, 255, 255}; - static constexpr Color magenta{255, 0, 255}; - static constexpr Color white{255, 255, 255}; - }; - - /** - * @brief Construct a new Argb object. - * - * @param config Configuration for the addressable RGB LED. - */ - explicit TArgb(const Config& config); - - /** - * @brief Set the color of the ARGB at the specified index. - * - * @param color The color to set the ARGB to. - * @param index The index of the ARGB to set the color of. - */ - void set_color(const Color& color, uint8_t index); - - /** - * @brief Set the color of all ARGBs. - * - * @param color The color to set all ARGBs to. - */ - void set_color(const Color& color); - - /** - * @brief Set the colors of all ARGBs. - * - * @param colors The colors to set the ARGBs to. - */ - void set_colors(const std::array& colors); - - /** - * @brief Turn off the ARGB at the specified index. - * - * @param index The index of the ARGB to turn off. - */ - void turn_off(uint8_t index); - - /** - * @brief Turn off all ARGBs. - */ - void turn_off(); - - /** - * @brief Send the colors to the addressable RGB LED. - */ - void update(); - - /** - * @brief Get the last color set on an ARGB. - * - * @param index The index of the ARGB. - * @return Last color set on the ARGB. - */ - Color get_color(uint8_t index) const; - -private: - /** - * @brief Publish the current colours to the boundary record. - */ - void publish() const; - - /** - * @brief Boundary record this proxy writes its colours into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Last colors set on the ARGBs. - */ - std::array colors{}; -}; -} // namespace micras::proxy - -#include "../src/proxy/argb.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_PROXY_ARGB_HPP diff --git a/include/micras/proxy/battery.hpp b/include/micras/proxy/battery.hpp deleted file mode 100644 index d7a3b24..0000000 --- a/include/micras/proxy/battery.hpp +++ /dev/null @@ -1,75 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_BATTERY_HPP -#define MICRAS_PROXY_BATTERY_HPP - -#include - -namespace micras::proxy { -/** - * @brief Battery stub reporting a constant, fully charged pack. - */ -class Battery { -public: - /** - * @brief Configuration struct for the battery. - */ - struct Config { - float max_voltage; - uint16_t max_reading; - }; - - /** - * @brief Construct a new Battery object. - * - * @param config Configuration for the battery. - */ - explicit Battery(const Config& config); - - /** - * @brief Update the battery reading. - */ - void update(); - - /** - * @brief Get the battery voltage. - * - * @return Battery voltage in volts. - */ - float get_voltage() const; - - /** - * @brief Get the battery reading from the ADC. - * - * @return Raw battery reading. - */ - float get_voltage_raw() const; - - /** - * @brief Get the battery reading from the ADC. - * - * @return Battery reading from 0 to 1. - */ - float get_adc_reading() const; - -private: - /** - * @brief Constant battery voltage in volts. - */ - float reading; - - /** - * @brief Max battery voltage for the adc conversion. - */ - float max_voltage; - - /** - * @brief Max adc reading for the battery. - */ - uint16_t max_reading; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_BATTERY_HPP diff --git a/include/micras/proxy/bluetooth_serial.hpp b/include/micras/proxy/bluetooth_serial.hpp deleted file mode 100644 index d7fdb57..0000000 --- a/include/micras/proxy/bluetooth_serial.hpp +++ /dev/null @@ -1,73 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_BLUETOOTH_SERIAL_HPP -#define MICRAS_PROXY_BLUETOOTH_SERIAL_HPP - -#include -#include - -#include "micras/sim/core/serial_bus.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief In-process replacement for the bluetooth serial link. - * - * @note Bytes are exchanged through sim::ProxyState so the harness can decode - * the firmware telemetry and inject command packets without any socket. - */ -class BluetoothSerial { -public: - /** - * @brief Configuration struct for the BluetoothSerial. - */ - struct Config { - /** - * @brief Simulation holding the serial bus this radio writes to. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new BluetoothSerial object. - * - * @param config Configuration for the BluetoothSerial. - */ - explicit BluetoothSerial(const Config& config); - - /** - * @brief Process the serial channel. Nothing is buffered, so this is a no-op. - */ - void update(); - - /** - * @brief Send data to the harness. - * - * @param data Bytes to be sent. - */ - void send_data(std::vector data); - - /** - * @brief Get the data queued by the harness since the last call. - * - * @return Vector containing received bytes, empty if there is no new data. - */ - std::vector get_data(); - -private: - /** - * @brief Byte channel shared with the harness. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::SerialBus& serial; // NOLINT(*-avoid-const-or-ref-data-members) -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_BLUETOOTH_SERIAL_HPP diff --git a/include/micras/proxy/button.hpp b/include/micras/proxy/button.hpp deleted file mode 100644 index 11b0956..0000000 --- a/include/micras/proxy/button.hpp +++ /dev/null @@ -1,162 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_BUTTON_HPP -#define MICRAS_PROXY_BUTTON_HPP - -#include -#include - -#include "micras/proxy/stopwatch.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Button driven by the scenario through sim::ProxyState. - */ -class Button { -public: - /** - * @brief Enum for button status. - */ - enum Status : uint8_t { - NO_PRESS = 0, - SHORT_PRESS = 1, - LONG_PRESS = 2, - EXTRA_LONG_PRESS = 3 - }; - - /** - * @brief Enum for button pull resistor. - */ - enum PullResistor : uint8_t { - PULL_UP = 0, - PULL_DOWN = 1, - }; - - /** - * @brief Configuration structure for button. - */ - struct Config { - std::string name; - uint16_t long_press_delay{500}; - uint16_t extra_long_press_delay{2000}; - /** - * @brief Simulation holding the state the button is read from. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new Button object. - * - * @param config Button configuration. - */ - explicit Button(const Config& config); - - /** - * @brief Check if button is pressed. - * - * @return True if button is pressed, false otherwise. - */ - bool is_pressed() const; - - /** - * @brief Get button status. - * - * @return Current button status. - */ - Status get_status() const; - - /** - * @brief Update the status of the button. - * - * @note A program that polls the button in a tight loop is waiting for a - * press only the simulation can deliver, so repeated updates at the - * same simulated time hand a tick over. The firmware calls this once - * per tick and therefore never reaches that point. - */ - void update(); - -private: - /** - * @brief Number of updates at the same simulated time that count as polling. - */ - static constexpr uint8_t max_repeated_updates{2}; - - /** - * @brief Simulation the state and the tick handoff come from. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::SimulationContext& simulation; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Simulated time of the last update. - * - * @note Seeded from the clock so the first update is never mistaken for a - * repeat of an update that never happened. - */ - uint64_t last_update_us{}; - - /** - * @brief Consecutive updates at the same simulated time. - */ - uint8_t repeated_updates{0}; - - /** - * @brief Update button state from the simulation. - */ - void update_state(); - - /** - * @brief Check if button was just pressed. - * - * @return True if button was just pressed, false otherwise. - */ - bool is_rising_edge() const; - - /** - * @brief Check if button was just released. - * - * @return True if button was just released, false otherwise. - */ - bool is_falling_edge() const; - - /** - * @brief Button pressing delays in ms. - */ - ///@{ - uint16_t long_press_delay; - uint16_t extra_long_press_delay; - ///@} - - /** - * @brief Stopwatch to determine type of button press. - */ - Stopwatch status_stopwatch; - - /** - * @brief Flag to know if button was being pressed. - */ - bool previous_state{false}; - - /** - * @brief Flag to know if button is being pressed. - */ - bool current_state{false}; - - /** - * @brief Current status of the button. - */ - Status current_status{NO_PRESS}; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_BUTTON_HPP diff --git a/include/micras/proxy/buzzer.hpp b/include/micras/proxy/buzzer.hpp deleted file mode 100644 index 37e11a6..0000000 --- a/include/micras/proxy/buzzer.hpp +++ /dev/null @@ -1,114 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_BUZZER_HPP -#define MICRAS_PROXY_BUZZER_HPP - -#include -#include - -#include "micras/proxy/stopwatch.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Stub buzzer that only records the last tone requested. - */ -class Buzzer { -public: - /** - * @brief Configuration struct for the buzzer. - */ - struct Config { - std::string name; - - /** - * @brief Simulation holding the state this buzzer is heard in. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new Buzzer object. - * - * @param config Configuration for the buzzer. - */ - explicit Buzzer(const Config& config); - - /** - * @brief Play a tone for a duration. - * - * @param frequency Buzzer sound frequency in Hz. - * @param duration Duration of the sound in ms. - */ - void play(uint32_t frequency, uint32_t duration = 0); - - /** - * @brief Update the buzzer state. - */ - void update(); - - /** - * @brief Stop the buzzer sound. - */ - void stop(); - - /** - * @brief Wait for a time interval. - * - * @note Blocking waits cannot advance simulated time, so this only stops the - * tone. It is never called by the firmware. - * - * @param interval Time to wait in ms. - */ - void wait(uint32_t interval); - - /** - * @brief Get the frequency currently being played. - * - * @return Frequency in Hz, zero when silent. - */ - uint32_t get_frequency() const; - -private: - /** - * @brief Publish the current tone to the boundary record. - */ - void publish() const; - - /** - * @brief Boundary record this proxy writes its tone into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Stopwatch to time the sound duration. - */ - Stopwatch stopwatch; - - /** - * @brief Frequency currently being played. - */ - uint32_t frequency{}; - - /** - * @brief Flag to check if the buzzer is playing. - */ - bool is_playing{}; - - /** - * @brief Duration of the sound in ms. - */ - uint32_t duration{}; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_BUZZER_HPP diff --git a/include/micras/proxy/dip_switch.hpp b/include/micras/proxy/dip_switch.hpp deleted file mode 100644 index 395fce4..0000000 --- a/include/micras/proxy/dip_switch.hpp +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_DIP_SWITCH_HPP -#define MICRAS_PROXY_DIP_SWITCH_HPP - -#include -#include -#include - -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief DIP switch driven by the scenario through sim::ProxyState. - */ -template -class TDipSwitch { -public: - /** - * @brief Configuration struct for the Dip Switch. - */ - struct Config { - std::array names; - /** - * @brief Simulation holding the state the switches are read from. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new Dip Switch object. - * - * @param config Configuration struct for the DipSwitch. - */ - explicit TDipSwitch(const Config& config); - - /** - * @brief Get the state of a switch. - * - * @param switch_index Index of the switch. - * @return True if the switch is on, false otherwise. - */ - bool get_switch_state(uint8_t switch_index) const; - - /** - * @brief Get the value of all switches. - * - * @return Value of all switches. - */ - uint8_t get_switches_value() const; - -private: - /** - * @brief State the scenario or the GUI writes the switches into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - const sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) -}; -} // namespace micras::proxy - -#include "../src/proxy/dip_switch.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_PROXY_DIP_SWITCH_HPP diff --git a/include/micras/proxy/fan.hpp b/include/micras/proxy/fan.hpp deleted file mode 100644 index c663254..0000000 --- a/include/micras/proxy/fan.hpp +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_FAN_HPP -#define MICRAS_PROXY_FAN_HPP - -#include - -#include "micras/proxy/stopwatch.hpp" -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Fan driver writing directly to a MuJoCo actuator control input. - * - * @note The speed ramp mirrors the firmware Fan: set_speed only moves the - * target and update() slews the current speed towards it at - * max_acceleration percent per millisecond of simulated time. - */ -class Fan { -public: - /** - * @brief Configuration struct for the fan. - */ - struct Config { - std::string actuator; - float max_acceleration; - /** - * @brief Simulation holding the actuator this fan drives. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new fan object. - * - * @param config Configuration for the fan driver. - */ - explicit Fan(const Config& config); - - /** - * @brief Enable the fan. - */ - void enable(); - - /** - * @brief Disable the fan. - */ - void disable(); - - /** - * @brief Set the target speed of the fans. - * - * @param speed Speed percentage of the fan. - */ - void set_speed(float speed); - - /** - * @brief Slew the current speed towards the target and drive the actuator. - * - * @note A zero speed parks the actuator instead of writing a zero duty - * cycle through the direction logic, matching the firmware. - * - * @return Current speed value. - */ - float update(); - - /** - * @brief Stop the fan, zeroing the actuator without clearing the target. - */ - void stop(); - -private: - /** - * @brief Simulation the actuator lives in. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Boundary record: the switch that silences the fan comes in here, - * the speed the fan settled on goes out. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Write the current speed to the actuator. - */ - void apply(); - - /** - * @brief Id of the MuJoCo actuator driven by this fan. - */ - int actuator_id; - - /** - * @brief Maximum acceleration of the fan in percentage per millisecond. - */ - float max_acceleration; - - /** - * @brief Current speed percentage written to the actuator. - */ - float current_speed{0.0F}; - - /** - * @brief Last requested speed percentage. - */ - float target_speed{0.0F}; - - /** - * @brief Flag to check if the fan is enabled. - */ - bool enabled{false}; - - /** - * @brief Stopwatch for limiting the acceleration of the fan. - */ - proxy::Stopwatch acceleration_stopwatch; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_FAN_HPP diff --git a/include/micras/proxy/imu.hpp b/include/micras/proxy/imu.hpp deleted file mode 100644 index 7d3651d..0000000 --- a/include/micras/proxy/imu.hpp +++ /dev/null @@ -1,136 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_IMU_HPP -#define MICRAS_PROXY_IMU_HPP - -#include -#include -#include - -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief IMU reading the MuJoCo gyro and accelerometer sensors. - * - * @note The imu site frame matches the robot body frame: +y forward, +z up, - * +x right, so the yaw rate is the gyro z component and the - * accelerometer reports +g on z while at rest. - */ -class Imu { -public: - /** - * @brief IMU configuration struct. - */ - struct Config { - std::string gyro_sensor; - std::string accelerometer_sensor; - /** - * @brief Simulation holding the sensors this IMU reads. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Enum to select the axis of the IMU. - */ - enum class Axis : uint8_t { - X = 0, - Y = 1, - Z = 2 - }; - - /** - * @brief Construct a new Imu object. - * - * @param config Configuration for the IMU. - */ - explicit Imu(const Config& config); - - /** - * @brief Update the IMU data. - */ - void update(); - - /** - * @brief Get the IMU angular velocity over an axis. - * - * @param axis Axis to get the angular velocity from. - * @return Angular velocity over the desired axis in rad/s. - */ - float get_angular_velocity(Axis axis) const; - - /** - * @brief Get the IMU linear acceleration over an axis. - * - * @param axis Axis to get the linear acceleration from. - * @return Linear acceleration over the desired axis in m/sยฒ. - */ - float get_linear_acceleration(Axis axis) const; - - /** - * @brief Define the base reading to be removed from the IMU value. - */ - void calibrate(); - - /** - * @brief Check if IMU was initialized. - * - * @return Always true in the simulation. - */ - bool was_initialized() const; - -private: - /** - * @brief Boundary record this proxy writes its readings into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Simulation the readings come from. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - const sim::MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Check the identity of the simulated device. - * - * @return Always true in the simulation. - */ - bool check_whoami(); - - /** - * @brief Address of the gyroscope values inside mjData::sensordata. - */ - int gyro_address; - - /** - * @brief Address of the accelerometer values inside mjData::sensordata. - */ - int accelerometer_address; - - /** - * @brief Current angular velocity on each axis. - */ - std::array angular_velocity{}; - - /** - * @brief Current linear acceleration on each axis. - */ - std::array linear_acceleration{}; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_IMU_HPP diff --git a/include/micras/proxy/led.hpp b/include/micras/proxy/led.hpp deleted file mode 100644 index 727e199..0000000 --- a/include/micras/proxy/led.hpp +++ /dev/null @@ -1,84 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_LED_HPP -#define MICRAS_PROXY_LED_HPP - -#include - -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Stub LED that only records its last state. - */ -class Led { -public: - /** - * @brief Configuration struct for LED. - */ - struct Config { - std::string name; - - /** - * @brief Simulation holding the state this LED is shown in. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new Led object. - * - * @param config Configuration for the LED. - */ - explicit Led(const Config& config); - - /** - * @brief Turn the LED on. - */ - void turn_on(); - - /** - * @brief Turn the LED off. - */ - void turn_off(); - - /** - * @brief Toggle the LED. - */ - void toggle(); - - /** - * @brief Get the current state of the LED. - * - * @return True if the LED is on, false otherwise. - */ - bool is_on() const; - -private: - /** - * @brief Publish the current state to the boundary record. - */ - void publish() const; - - /** - * @brief Boundary record this proxy writes what it shows into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Current state of the LED. - */ - bool state_on{false}; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_LED_HPP diff --git a/include/micras/proxy/locomotion.hpp b/include/micras/proxy/locomotion.hpp deleted file mode 100644 index fcd2940..0000000 --- a/include/micras/proxy/locomotion.hpp +++ /dev/null @@ -1,97 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_LOCOMOTION_HPP -#define MICRAS_PROXY_LOCOMOTION_HPP - -#include "micras/proxy/motor.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::proxy { -/** - * @brief Class for controlling the locomotion driver. - */ -class Locomotion { -public: - /** - * @brief Configuration struct for the locomotion. - */ - struct Config { - Motor::Config left_motor; - Motor::Config right_motor; - - /** - * @brief Simulation holding the state the commands are recorded into. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new locomotion object. - * - * @param config Configuration for the locomotion driver. - */ - explicit Locomotion(const Config& config); - - /** - * @brief Enable the locomotion driver. - */ - void enable(); - - /** - * @brief Disable the locomotion driver. - */ - void disable(); - - /** - * @brief Set the command of the wheels. - * - * @note Mirrors the firmware enable_gpio semantics: while the driver is - * disabled both actuators are driven to zero whatever is requested. - * - * @param left_command Command of the left wheels. - * @param right_command Command of the right wheels. - */ - void set_wheel_command(float left_command, float right_command); - - /** - * @brief Set the linear and angular commands of the robot. - * - * @param linear Linear command of the robot. - * @param angular Angular command of the robot. - */ - void set_command(float linear, float angular); - - /** - * @brief Stop the motors. - */ - void stop(); - -private: - /** - * @brief Boundary record this proxy writes its wheel commands into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Left motor of the robot. - */ - Motor left_motor; - - /** - * @brief Right motor of the robot. - */ - Motor right_motor; - - /** - * @brief Flag to indicate if the locomotion is enabled. - */ - bool enabled{false}; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_LOCOMOTION_HPP diff --git a/include/micras/proxy/motor.hpp b/include/micras/proxy/motor.hpp deleted file mode 100644 index bda0ab5..0000000 --- a/include/micras/proxy/motor.hpp +++ /dev/null @@ -1,69 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_MOTOR_HPP -#define MICRAS_PROXY_MOTOR_HPP - -#include - -#include "micras/sim/core/mujoco_world.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Motor driver writing directly to a MuJoCo actuator control input. - */ -class Motor { -public: - /** - * @brief Configuration struct for the motor. - */ - struct Config { - std::string actuator; - /** - * @brief Simulation holding the actuator this motor drives. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new motor object. - * - * @param config Configuration for the motor driver. - */ - explicit Motor(const Config& config); - - /** - * @brief Set the command for the motor. - * - * @note The firmware deadzone and max_stopped_command remap is deliberately - * not mirrored here: it compensates for real H-bridge and gearbox - * stiction, below which the physical motor does not turn at all. The - * MuJoCo actuator is an ideal torque source with no deadzone, so the - * remap would inject an offset the simulated plant never needed. - * - * @param command Command for the motor in percentage. - */ - void set_command(float command); - -private: - /** - * @brief Simulation the actuator lives in. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Id of the MuJoCo actuator driven by this motor. - */ - int actuator_id; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_MOTOR_HPP diff --git a/include/micras/proxy/rotary_sensor.hpp b/include/micras/proxy/rotary_sensor.hpp deleted file mode 100644 index cc69d14..0000000 --- a/include/micras/proxy/rotary_sensor.hpp +++ /dev/null @@ -1,82 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_ROTARY_SENSOR_HPP -#define MICRAS_PROXY_ROTARY_SENSOR_HPP - -#include - -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Rotary sensor reading a MuJoCo jointpos sensor. - */ -class RotarySensor { -public: - /** - * @brief Rotary sensor configuration struct. - */ - struct Config { - std::string sensor; - - /** - * @brief Wheel this encoder belongs to, left first. - */ - std::size_t wheel{0}; - /** - * @brief Simulation holding the sensor this encoder reads. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new RotarySensor object. - * - * @param config Configuration for the rotary sensor. - */ - explicit RotarySensor(const Config& config); - - /** - * @brief Get the rotary sensor position over an axis. - * - * @return Current angular position of the sensor in radians. - */ - float get_position() const; - -private: - /** - * @brief Boundary record this proxy writes its reading into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Wheel this encoder belongs to. - */ - std::size_t wheel; - - /** - * @brief Simulation the readings come from. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - const sim::MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Address of the sensor value inside mjData::sensordata. - */ - int sensor_address; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_ROTARY_SENSOR_HPP diff --git a/include/micras/proxy/stopwatch.hpp b/include/micras/proxy/stopwatch.hpp deleted file mode 100644 index fedc964..0000000 --- a/include/micras/proxy/stopwatch.hpp +++ /dev/null @@ -1,179 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_STOPWATCH_HPP -#define MICRAS_PROXY_STOPWATCH_HPP - -#include - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Class to measure the time elapsed between two events. - * - * @note Backed by the simulated clock, never by wall time. A freshly built - * stopwatch reports one loop period rather than zero, so the firmware's - * very first loop sees the nominal period instead of a division by an - * uninitialised timer. At time zero that seed wraps the unsigned counter, - * which is well defined and gives exactly the intended reading. - */ -class Stopwatch { -public: - /** - * @brief Stopwatch configuration struct. - */ - struct Config { - /** - * @brief Simulation holding the clock this stopwatch reads. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new Stopwatch object on the process-wide simulation. - * - * @note The firmware default-constructs stopwatches inside its own headers - * (states/wait.hpp), where no Config can be threaded through, so this - * constructor delegates to the one target.hpp declares. Anything that - * has a Config should pass it instead, so that a test running on its - * own SimulationContext is honoured. - */ - Stopwatch(); - - /** - * @brief Construct a new Stopwatch object. - * - * @param config Configuration for the timer. - */ - explicit Stopwatch(const Config& config); - - /** - * @brief Reset the milliseconds timer counter. - */ - void reset_ms(); - - /** - * @brief Reset the microseconds timer counter. - */ - void reset_us(); - - /** - * @brief Get the simulated time elapsed since the last reset. - * - * @note Like the microsecond reading, a program that keeps asking at the - * same simulated instant is waiting for time it cannot get any other - * way, so it is handed a tick. The threshold is higher here because - * the firmware legitimately reads the same stopwatch twice in a tick - * when it classifies a button release. - * - * @return Time elapsed in milliseconds. - */ - uint32_t elapsed_time_ms() const; - - /** - * @brief Get the simulated time elapsed since the last reset. - * - * @note This is the one place where the simulated clock is not a pure - * observation: a program that keeps reading the same instant is - * waiting for time that only the simulation can give it, so the - * repeated read hands a tick over. The reading itself is always - * honest, now minus the last reset; a short measurement stays short. - * - * @return Time elapsed in microseconds. - */ - uint32_t elapsed_time_us() const; - - /** - * @brief Sleep on the process-wide simulation. - * - * @note Static in the firmware, and the hardware tests call it that way, so - * it reaches the same context the default constructor does. Returns - * at once when no program is being driven. - * - * @param time Time to sleep in milliseconds. - */ - static void sleep_ms(uint32_t time); - - /** - * @brief Hand ticks over until the simulated clock reaches the target. - * - * @note Returns at once when no program is being driven, since there would - * be nothing to hand the tick to. - * - * @param time Time to sleep in microseconds. - */ - void sleep_us(uint32_t time) const; - -private: - /** - * @brief Hand the tick over to the simulation, if a program is running. - * - * @return False when no tick was handed over, so a wait knows to give up. - */ - bool yield_tick() const; - - /** - * @brief Simulation the readings and the handoff come from. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::SimulationContext& simulation; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Number of microsecond reads at the same simulated time that count as a spin. - * - * @note Two, because a firmware loop legitimately reads the same instant - * twice: once to close the previous loop and once to open the next. - * A third read means the program is waiting for time to pass. - */ - static constexpr uint8_t max_repeated_reads{2}; - - /** - * @brief Number of millisecond reads at the same simulated time that count as a spin. - * - * @note Four: no firmware path reads one stopwatch in milliseconds more - * than twice within a tick, and the margin keeps the guard clear of - * any legitimate pattern. - */ - static constexpr uint8_t max_repeated_ms_reads{4}; - - /** - * @brief Simulated time in microseconds at the last reset. - */ - uint64_t counter{}; - - /** - * @brief Simulated time of the last reading. - * - * @note Not touched by a reset: the guard measures how many times a program - * asked at the same instant, and a proxy that resets its own - * stopwatch on every call, as the fan does, must not be able to hide - * a spin behind it. - */ - mutable uint64_t last_read_us{}; - - /** - * @brief Forget both spin counters after a tick was handed over. - * - * @note Both, because the two readings share the instant they compare - * against: leaving one behind would make a stopwatch read in both - * units interfere with itself. - */ - void note_yield() const; - - /** - * @brief Consecutive readings taken at the same simulated time. - */ - ///@{ - mutable uint8_t repeated_reads{0}; - mutable uint8_t repeated_ms_reads{0}; - ///@} -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_STOPWATCH_HPP diff --git a/include/micras/proxy/storage.hpp b/include/micras/proxy/storage.hpp deleted file mode 100644 index 224332d..0000000 --- a/include/micras/proxy/storage.hpp +++ /dev/null @@ -1,174 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_STORAGE_HPP -#define MICRAS_PROXY_STORAGE_HPP - -#include -#include -#include -#include -#include - -#include "micras/core/serializable.hpp" - -namespace micras::proxy { -template -concept Fundamental = std::is_fundamental_v; - -/** - * @brief Non-volatile storage emulated with an in-memory page map. - * - * @note The page map lives for the duration of the process, so every run starts - * from a blank flash. No file is ever touched. - */ -class Storage { -public: - /** - * @brief Configuration struct for the storage. - */ - struct Config { - uint16_t start_page; - uint16_t number_of_pages; - }; - - /** - * @brief Construct a new Storage object. - * - * @param config Configuration for the storage. - */ - explicit Storage(const Config& config); - - /** - * @brief Create a new primitive variable in the storage. - * - * @tparam T Type of the variable. - * @param name Name of the variable. - * @param data Reference to the variable. - */ - template - void create(const std::string& name, const T& data) { - this->primitives[name].ram_pointer = &data; - this->primitives.at(name).size = sizeof(T); - } - - /** - * @brief Create a new serializable variable in the storage. - * - * @param name Name of the variable. - * @param data Reference to the variable. - */ - void create(const std::string& name, const core::ISerializable& data); - - /** - * @brief Sync a primitive variable with the storage. - * - * @tparam T Type of the variable. - * @param name Name of the variable. - * @param data Reference to the variable. - */ - template - void sync(const std::string& name, T& data) { - if (this->primitives.contains(name) and this->primitives.at(name).ram_pointer == nullptr) { - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - data = reinterpret_cast(this->buffer.at(this->primitives.at(name).buffer_address)); - } - - this->create(name, data); - } - - /** - * @brief Sync a serializable variable with the storage. - * - * @param name Name of the variable. - * @param data Reference to the variable. - */ - void sync(const std::string& name, core::ISerializable& data); - - /** - * @brief Save the storage to the emulated flash. - */ - void save(); - -private: - /** - * @brief Struct for primitive variables. - */ - struct PrimitiveVariable { - const void* ram_pointer{nullptr}; - uint16_t buffer_address{}; - uint16_t size{}; - }; - - /** - * @brief Struct for serializable variables. - */ - struct SerializableVariable { - const core::ISerializable* ram_pointer{nullptr}; - uint16_t buffer_address{}; - uint16_t size{}; - }; - - /** - * @brief Serialize a map of variables. - * - * @tparam T Type of the variables. - * @param variables Map of variables. - * @return Serialized buffer. - */ - template - static std::vector serialize_var_map(const std::unordered_map& variables); - - /** - * @brief Deserialize a map of variables. - * - * @tparam T Type of the variables. - * @param buffer Serialized buffer. - * @param num_vars Number of variables. - * @return Deserialized map of variables. - */ - template - static std::unordered_map deserialize_var_map(std::vector& buffer, uint16_t num_vars); - - /** - * @brief Emulated flash pages, keyed by start page. - */ - static std::unordered_map>& pages(); - - /** - * @brief Start symbol to avoid reading garbage from flash. - */ - static constexpr uint16_t start_symbol = 0xABAB; - - /** - * @brief Map of primitive variables. - */ - std::unordered_map primitives; - - /** - * @brief Map of serializable variables. - */ - std::unordered_map serializables; - - /** - * @brief Serialized buffer for the storage. - */ - std::vector buffer; - - /** - * @brief Start page of the storage in the emulated flash memory. - */ - uint16_t start_page; - - /** - * @brief Maximum number of pages used by the storage in the emulated flash memory. - * - * @note Kept to mirror the firmware Config; the simulated storage lives - * in memory and is not page limited. - */ - [[maybe_unused]] uint16_t number_of_pages; -}; -} // namespace micras::proxy - -#endif // MICRAS_PROXY_STORAGE_HPP diff --git a/include/micras/proxy/torque_sensors.hpp b/include/micras/proxy/torque_sensors.hpp deleted file mode 100644 index 285d2ed..0000000 --- a/include/micras/proxy/torque_sensors.hpp +++ /dev/null @@ -1,87 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_TORQUE_SENSORS_HPP -#define MICRAS_PROXY_TORQUE_SENSORS_HPP - -#include - -namespace micras::proxy { -/** - * @brief Torque sensors stub. Every reading is zero. - */ -template -class TTorqueSensors { -public: - /** - * @brief Configuration struct for torque sensors. - */ - struct Config { - float shunt_resistor; - float max_torque; - float reference_voltage; - }; - - /** - * @brief Construct a new TorqueSensors object. - * - * @param config Configuration for the torque sensors. - */ - explicit TTorqueSensors(const Config& config); - - /** - * @brief Calibrate the torque sensors. - */ - void calibrate(); - - /** - * @brief Update the torque sensors readings. - */ - void update(); - - /** - * @brief Get the torque from the sensor. - * - * @param sensor_index Index of the sensor. - * @return Torque reading from the sensor in N*m. - */ - float get_torque(uint8_t sensor_index) const; - - /** - * @brief Get the raw torque from the sensor without filtering. - * - * @param sensor_index Index of the sensor. - * @return Raw torque reading from the sensor. - */ - float get_torque_raw(uint8_t sensor_index) const; - - /** - * @brief Get the electric current through the sensor. - * - * @param sensor_index Index of the sensor. - * @return Current reading from the sensor in amps. - */ - float get_current(uint8_t sensor_index) const; - - /** - * @brief Get the raw electric current through the sensor without filtering. - * - * @param sensor_index Index of the sensor. - * @return Raw current reading from the sensor in amps. - */ - float get_current_raw(uint8_t sensor_index) const; - - /** - * @brief Get the ADC reading from the sensor. - * - * @param sensor_index Index of the sensor. - * @return Adc reading from the sensor from 0 to 1. - */ - float get_adc_reading(uint8_t sensor_index) const; -}; -} // namespace micras::proxy - -#include "../src/proxy/torque_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_PROXY_TORQUE_SENSORS_HPP diff --git a/include/micras/proxy/wall_sensors.hpp b/include/micras/proxy/wall_sensors.hpp deleted file mode 100644 index 486e22e..0000000 --- a/include/micras/proxy/wall_sensors.hpp +++ /dev/null @@ -1,181 +0,0 @@ -/** - * @file - */ - -#ifndef MICRAS_PROXY_WALL_SENSORS_HPP -#define MICRAS_PROXY_WALL_SENSORS_HPP - -#include -#include -#include - -#include "micras/core/butterworth_filter.hpp" -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/core/proxy_state.hpp" - -namespace micras::sim { -struct SimulationContext; -} // namespace micras::sim - -namespace micras::proxy { -/** - * @brief Wall sensors backed by MuJoCo rangefinder sensors. - */ -template -class TWallSensors { -public: - /** - * @brief Configuration struct for wall sensors. - * - * @note No member carries a default: every field is required from - * target.hpp, so a missing one is a compile error rather than a - * silent zero. - */ - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-member-init) - struct Config { - std::array sensors; - float uncertainty; - std::array base_readings; - float max_sensor_reading; - float min_sensor_reading; - float max_sensor_distance; - float filter_cutoff; - float sampling_frequency; - /** - * @brief Simulation holding the rangefinders these sensors read. - */ - sim::SimulationContext* context{nullptr}; - }; - - /** - * @brief Construct a new WallSensors object. - * - * @param config Configuration for the wall sensors. - */ - explicit TWallSensors(const Config& config); - - /** - * @brief Turn on the wall sensors IR LED. - */ - void turn_on(); - - /** - * @brief Turn off the wall sensors IR LED. - */ - void turn_off(); - - /** - * @brief Update the wall sensors readings. - */ - void update(); - - /** - * @brief Get the observation from a sensor. - * - * @param sensor_index Index of the sensor. - * @param disturbed Whether or not there is another wall perpendicular to the one being measured. - * @return True if the sensor detects a wall, false otherwise. - */ - bool get_wall(uint8_t sensor_index, bool disturbed = false) const; - - /** - * @brief Get the reading from a sensor. - * - * @param sensor_index Index of the sensor. - * @return Reading from the sensor. - */ - float get_reading(uint8_t sensor_index) const; - - /** - * @brief Get the ADC reading from a sensor. - * - * @note A MuJoCo rangefinder returns -1 when its ray hits nothing, which - * maps to no reflection at all, and a zero distance would make the - * intensity infinite, so it saturates instead. - * - * @param sensor_index Index of the sensor. - * @return ADC reading from the sensor from 0 to 1. - */ - float get_adc_reading(uint8_t sensor_index) const; - - /** - * @brief Get the deviation of a wall sensor reading from its calibrated baseline. - * - * @param sensor_index Index of the sensor. - * @return The reading error relative to the baseline; positive if above baseline. - */ - float get_sensor_error(uint8_t sensor_index) const; - - /** - * @brief Calibrate a wall sensor base reading. - * - * @param sensor_index Index of the sensor. - */ - void calibrate_sensor(uint8_t sensor_index); - -private: - /** - * @brief Publish an unfiltered reading and hand it back to the caller. - * - * @param sensor_index Index of the sensor. - * @param reading Reading to publish. - * @return The same reading. - */ - float record(uint8_t sensor_index, float reading) const; - - /** - * @brief Boundary record this proxy writes its unfiltered readings into. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - sim::ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Simulation the readings come from. - * - * @note Bound for the life of the proxy; a pointer would only add a null - * check to every access. - */ - const sim::MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Addresses of the rangefinder values inside mjData::sensordata. - */ - std::array sensor_addresses{}; - - /** - * @brief Maximum reading detectable by the sensor. - */ - float max_sensor_reading; - - /** - * @brief LED state. - */ - bool leds_on{false}; - - /** - * @brief Butterworth filter for the ADC readings. - */ - std::array filters; - - /** - * @brief Measured wall values during calibration. - */ - std::array base_readings; - - /** - * @brief Ratio of the base reading to still consider as seeing a wall. - */ - float uncertainty; - - /** - * @brief Constant value for the wall sensor distance to reading conversion. - */ - float constant; -}; -} // namespace micras::proxy - -#include "../src/proxy/wall_sensors.cpp" // NOLINT(bugprone-suspicious-include, misc-header-include-cycle) - -#endif // MICRAS_PROXY_WALL_SENSORS_HPP diff --git a/include/micras/sim/app/application.hpp b/include/micras/sim/app/application.hpp deleted file mode 100644 index 80de85d..0000000 --- a/include/micras/sim/app/application.hpp +++ /dev/null @@ -1,144 +0,0 @@ -/** - * @file - * - * @brief Wires a simulation together and runs one program in it. - */ - -#ifndef MICRAS_SIM_APP_APPLICATION_HPP -#define MICRAS_SIM_APP_APPLICATION_HPP - -#include -#include - -#include "micras/sim/app/cli.hpp" -#include "micras/sim/bridge/monitor_bridge.hpp" -#include "micras/sim/core/firmware_thread.hpp" -#include "micras/sim/core/scenario.hpp" -#include "micras/sim/core/simulation.hpp" -#include "micras/sim/core/simulation_context.hpp" -#include "micras/sim/recording/csv_recorder.hpp" -#include "micras/sim/telemetry/telemetry.hpp" -#include "micras/sim/view/mujoco_viewer.hpp" -#include "micras/sim/view/video_recorder.hpp" - -namespace micras::sim { -/** - * @brief Whether the program being run serves the firmware variable pool. - */ -enum class PoolTelemetry : uint8_t { - SERVED, - NOT_SERVED, -}; - -/** - * @brief Builds the context, the listeners and the program, then runs them. - * - * @note This is the only place besides config/target.hpp that names the - * process-wide SimulationContext, and the only place that decides what a - * run is made of. Everything it builds is a listener of one Simulation. - */ -class Application { -public: - /** - * @brief Set a simulation up for a parsed command line. - * - * @note The variable map is requested here rather than during the run, - * because the firmware answers it on the very first tick and the CSV - * header has to know every pool column before the first row. - * - * @param options Parsed command line. - * @param program Program to run in lockstep, typically the firmware loop. - * @param pool Whether the program answers variable pool requests. - */ - Application(const CliOptions& options, FirmwareThread::Program program, PoolTelemetry pool); - - Application(const Application&) = delete; - Application(Application&&) = delete; - Application& operator=(const Application&) = delete; - Application& operator=(Application&&) = delete; - - /** - * @brief Detach the program from the process-wide context. - */ - ~Application(); - - /** - * @brief Run the whole program and write the run directory. - * - * @param arguments Whole command line, recorded in the metadata. - */ - void run(std::span arguments); - - /** - * @brief Parse, build and run, reporting any failure on stderr. - * - * @param arguments Whole command line, program name included. - * @param program Program to run in lockstep. - * @param pool Whether the program answers variable pool requests. - * @return Process exit code. - */ - static int main( - std::span arguments, const FirmwareThread::Program& program, PoolTelemetry pool = PoolTelemetry::SERVED - ); - -private: - /** - * @brief Parsed command line. - */ - CliOptions options; - - /** - * @brief Simulation everything is wired into. - * - * @note Bound for the life of the application; the context outlives main. - */ - SimulationContext& context; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Program running in lockstep with the physics. - */ - FirmwareThread firmware; - - /** - * @brief Whether the program answers variable pool requests. - */ - PoolTelemetry pool; - - /** - * @brief Decoder of everything the firmware writes. - */ - Telemetry telemetry; - - /** - * @brief Scripted input for the run. - */ - Scenario scenario; - - /** - * @brief Writer of data.csv. - */ - CsvRecorder recorder; - - /** - * @brief Offscreen recorder, null unless --video asked for one and it came up. - */ - std::unique_ptr video; - - /** - * @brief Live window, null unless --viewer asked for one and it came up. - */ - std::unique_ptr viewer; - - /** - * @brief Bridge to micras-monitor, null unless --monitor asked for one. - */ - std::unique_ptr monitor; - - /** - * @brief The run itself. - */ - Simulation simulation; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_APP_APPLICATION_HPP diff --git a/include/micras/sim/core/mujoco_world.hpp b/include/micras/sim/core/mujoco_world.hpp deleted file mode 100644 index 3c49984..0000000 --- a/include/micras/sim/core/mujoco_world.hpp +++ /dev/null @@ -1,162 +0,0 @@ -/** - * @file - * - * @brief Owner of the MuJoCo model and data. - */ - -#ifndef MICRAS_SIM_CORE_MUJOCO_WORLD_HPP -#define MICRAS_SIM_CORE_MUJOCO_WORLD_HPP - -#include -#include - -#include - -namespace micras::sim { -/** - * @brief Loads a model, owns its data and resolves model objects by name. - * - * @note Every accessor throws a clear error when the model is not loaded yet, - * so a proxy built too early fails at construction instead of - * dereferencing a null pointer in the middle of a run. - */ -class MujocoWorld { -public: - MujocoWorld() = default; - - MujocoWorld(const MujocoWorld&) = delete; - MujocoWorld(MujocoWorld&&) = delete; - MujocoWorld& operator=(const MujocoWorld&) = delete; - MujocoWorld& operator=(MujocoWorld&&) = delete; - - /** - * @brief Free the MuJoCo model and data. - */ - ~MujocoWorld(); - - /** - * @brief Load a model and allocate its data, replacing any previous one. - * - * @note Automatic reset is disabled on the loaded model so a diverging step - * is reported instead of silently restarting the run. - * - * @param model_path Path to the model XML file. - */ - void load(const std::filesystem::path& model_path); - - /** - * @brief Check whether a model is loaded. - * - * @return True once load() succeeded. - */ - bool is_loaded() const { return this->model_handle != nullptr; } - - /** - * @brief Reset the simulation to its initial state and make sensordata valid. - */ - void reset(); - - /** - * @brief Advance the physics by a number of steps. - * - * @param steps Number of mj_step calls. - */ - void step(int steps); - - /** - * @brief Resolve a named MuJoCo object, throwing if it does not exist. - * - * @param type MuJoCo object type. - * @param name Name of the object. - * @return Id of the object. - */ - int require_id(mjtObj type, const std::string& name) const; - - /** - * @brief Resolve the sensordata address of a named sensor. - * - * @param name Name of the sensor. - * @return Index into mjData::sensordata. - */ - int sensor_address(const std::string& name) const; - - /** - * @brief Read one sensordata entry. - * - * @param address Index into mjData::sensordata. - * @return Value of the sensor channel. - */ - double sensor_value(int address) const; - - /** - * @brief Write one actuator control value. - * - * @param actuator_id Id of the actuator. - * @param value Control value. - */ - void set_control(int actuator_id, double value); - - /** - * @brief Get the timestep the model integrates with. - * - * @return Timestep in seconds. - */ - double timestep() const; - - /** - * @brief Get the simulated time of the loaded data. - * - * @return Simulated seconds since the last reset. - */ - double time() const; - - /** - * @brief Get the loaded model. - * - * @note Exposed for the recorder and the renderer, which need the whole - * model; the proxies use the named accessors instead. - * - * @return Pointer to the model. - */ - ///@{ - const mjModel* model() const; - mjModel* model(); - ///@} - - /** - * @brief Get the simulation data. - * - * @note Exposed for the recorder and the renderer, which need the whole - * state; the proxies use the named accessors instead. - * - * @return Pointer to the data. - */ - ///@{ - const mjData* data() const; - mjData* data(); - ///@} - -private: - /** - * @brief Throw if no model is loaded. - */ - void require_loaded() const; - - /** - * @brief Release the model and data, if any. - */ - void unload(); - - /** - * @brief Loaded MuJoCo model. - */ - mjModel* model_handle{nullptr}; - - /** - * @brief Simulation data of the loaded model. - */ - mjData* data_handle{nullptr}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_CORE_MUJOCO_WORLD_HPP diff --git a/include/micras/sim/core/proxy_state.hpp b/include/micras/sim/core/proxy_state.hpp deleted file mode 100644 index 08de63a..0000000 --- a/include/micras/sim/core/proxy_state.hpp +++ /dev/null @@ -1,200 +0,0 @@ -/** - * @file - * - * @brief Everything that crosses the proxy boundary, as a second source of truth. - */ - -#ifndef MICRAS_SIM_CORE_PROXY_STATE_HPP -#define MICRAS_SIM_CORE_PROXY_STATE_HPP - -#include -#include -#include - -namespace micras::sim { -/** - * @brief Human controls the proxies read, written by the scenario or the GUI. - * - * @note Same vocabulary as micras::Interface: the DIP switches are indexed as - * in Interface::DipSwitchPins (fan, diagonal, boost, risky). - */ -struct InterfaceInput { - /** - * @brief Number of DIP switches on the board. - */ - static constexpr std::size_t dip_switch_count{4}; - - /** - * @brief True while the button is held down. - */ - bool button_pressed{false}; - - /** - * @brief State of each DIP switch. - */ - std::array dip_switches{}; - - /** - * @brief Whether a human has taken the board over from the script. - * - * @note Set by the panel on the first control it is given, and never - * cleared: from that moment the scenario stops writing, so the two - * never fight over the same field and the run is marked interactive. - */ - bool driven_by_human{false}; -}; - -/** - * @brief One addressable LED, as the firmware set it. - */ -struct Rgb { - uint8_t red{0}; - uint8_t green{0}; - uint8_t blue{0}; - - bool operator==(const Rgb&) const = default; -}; - -/** - * @brief What the board is showing, written by the proxies a human watches. - * - * @note Same vocabulary as micras::Interface: this is the other half of - * InterfaceInput, the side the firmware drives rather than reads. - */ -struct InterfaceOutput { - /** - * @brief Number of addressable LEDs on the board. - */ - static constexpr std::size_t argb_count{2}; - - /** - * @brief Whether the plain LED is lit. - */ - bool led_on{false}; - - /** - * @brief Colour of each addressable LED. - */ - std::array argb{}; - - /** - * @brief Buzzer tone in hertz, zero while it is silent. - */ - uint32_t buzzer_frequency{0}; -}; - -/** - * @brief Harness switches that sit behind the firmware's back. - * - * @note Not part of the board: these are simulation-only overrides, kept apart - * from InterfaceInput so that section keeps matching micras::Interface. - */ -struct Overrides { - /** - * @brief Whether the Fan proxy is allowed to drive its actuator. - */ - bool fan_enabled{true}; -}; - -/** - * @brief Commands the firmware handed to the actuator proxies. - * - * @note Unlike the sensor block, these are aligned with the ground truth of the - * same CSV row: the firmware writes them during the tick whose physics - * then consumes them. - */ -struct Actuators { - /** - * @brief Wheel commands as Locomotion passed them to the motors, in percent. - */ - ///@{ - float left_command{0.0F}; - float right_command{0.0F}; - ///@} - - /** - * @brief Speed the Fan proxy last settled on, in percent. - */ - float fan_speed{0.0F}; -}; - -/** - * @brief Readings the sensor proxies handed to the firmware. - * - * @note These duplicate the ground truth on purpose: a disagreement between - * what the robot physically did and what its sensors reported is the - * signal. - * - * @note They are one tick behind the ground truth of the same CSV row. The - * firmware body runs before the physics of its tick, so the values it - * read describe the world at the end of the previous tick, exactly as a - * real robot acts on the reading it took a loop ago. Comparing a sensor - * column against the ground truth of the same row measures that lag, not - * a sensor error; shift one row first. - */ -struct Sensors { - /** - * @brief Number of wheels, wall sensors and inertial axes. - */ - ///@{ - static constexpr std::size_t wheel_count{2}; - static constexpr std::size_t wall_sensor_count{4}; - static constexpr std::size_t axis_count{3}; - ///@} - - /** - * @brief Encoder positions, left then right, in radians. - */ - std::array encoder_positions{}; - - /** - * @brief Gyroscope reading per axis, in radians per second. - */ - std::array angular_velocity{}; - - /** - * @brief Accelerometer reading per axis, in metres per second squared. - */ - std::array linear_acceleration{}; - - /** - * @brief Unfiltered wall sensor readings, the ones post detection uses. - */ - std::array wall_adc_readings{}; -}; - -/** - * @brief State the proxies expose to the harness, one writer per section. - * - * @note This is the second source of truth, independent of the firmware - * telemetry: it records what actually crossed the proxy boundary. - */ -struct ProxyState { - /** - * @brief Written by the scenario or the GUI, read by the Button and DipSwitch proxies. - */ - InterfaceInput interface_input; - - /** - * @brief Written by the Led, Argb and Buzzer proxies, read by the panel and the recorder. - */ - InterfaceOutput interface_output; - - /** - * @brief Written by the command line, read by the proxies it silences. - */ - Overrides overrides; - - /** - * @brief Written by the Locomotion and Fan proxies, read by the recorder. - */ - Actuators actuators; - - /** - * @brief Written by the sensor proxies, read by the recorder. - */ - Sensors sensors; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_CORE_PROXY_STATE_HPP diff --git a/include/micras/sim/core/scenario.hpp b/include/micras/sim/core/scenario.hpp deleted file mode 100644 index 2c28e30..0000000 --- a/include/micras/sim/core/scenario.hpp +++ /dev/null @@ -1,215 +0,0 @@ -/** - * @file - * - * @brief Scripted input for a run: what the robot is told to do, and when. - */ - -#ifndef MICRAS_SIM_CORE_SCENARIO_HPP -#define MICRAS_SIM_CORE_SCENARIO_HPP - -#include -#include -#include -#include -#include - -#include "micras/sim/core/proxy_state.hpp" -#include "micras/sim/core/simulation.hpp" -#include "micras/sim/telemetry/telemetry.hpp" - -namespace micras::sim { -/** - * @brief Command to hand the firmware during a run. - * - * @note Delivered as a single SERIAL_VARIABLE packet writing true to the - * matching bidirectional pool variable, exactly like the monitor does. - */ -enum class Command : uint8_t { - NONE, - EXPLORE, - SOLVE, - CALIBRATE, -}; - -/** - * @brief Button press to emit during a run. - */ -enum class ButtonPress : uint8_t { - NONE, - SHORT, - LONG, - EXTRA_LONG, -}; - -/** - * @brief Press durations the board classifies against. - * - * @note Taken from the Button config rather than read from target.hpp, so the - * scenario stays independent of the firmware shadow. - */ -struct ButtonDelays { - uint32_t long_press; - uint32_t extra_long_press; -}; - -/** - * @brief Everything the scenario can be told from the command line. - */ -struct ScenarioScript { - /** - * @brief Command to inject, and when. - */ - ///@{ - Command command{Command::NONE}; - double command_at{0.5}; - ///@} - - /** - * @brief Button press to emit, and when it goes down. - */ - ///@{ - ButtonPress button{ButtonPress::NONE}; - double button_at{0.5}; - ///@} - - /** - * @brief DIP switch states, indexed as in Interface::DipSwitchPins. - */ - std::array dip{}; - - /** - * @brief Whether the Fan proxy is allowed to drive its actuator. - */ - bool fan_enabled{true}; -}; - -/** - * @brief Parse a command name as the command line spells it. - * - * @param name One of none, explore, solve or calibrate. - * @return The command, or nullopt for an unknown name. - */ -std::optional parse_command(std::string_view name); - -/** - * @brief Parse a button press name as the command line spells it. - * - * @param name One of none, short, long or extra_long. - * @return The press, or nullopt for an unknown name. - */ -std::optional parse_button_press(std::string_view name); - -/** - * @brief Get the pool variable a command is delivered through. - * - * @param command Command to deliver. - * @return Firmware variable name, empty for Command::NONE. - */ -std::string_view command_variable(Command command); - -/** - * @brief Plays the script into the simulation, one tick at a time. - * - * @note Writes only interface input and the serial queue, never mjData or the - * clock, so a run driven by a scenario is the same run a human could - * drive by hand. - * - * @note Stops writing the board entirely once a human has touched it, so the - * two never fight over the same field. - */ -class Scenario : public IRunListener { -public: - /** - * @brief Take the script and everything it needs to play it. - * - * @param script What to do and when. - * @param state Interface input the button is written into. - * @param telemetry Decoder used to address the command variable. - * @param clock Clock the instants are converted against. - * @param delays Press durations the board classifies against. - */ - Scenario( - const ScenarioScript& script, ProxyState& state, Telemetry& telemetry, const Clock& clock, - const ButtonDelays& delays - ); - - /** - * @brief Apply the DIP switches and the fan override before the first tick. - * - * @param simulation Run about to start. - */ - void on_start(const Simulation& simulation) override; - - /** - * @brief Hold the button and inject the command at their instants. - * - * @param simulation Run about to advance. - * @return Always RunControl::RUN; a script never stops a run. - */ - RunControl on_before_tick(const Simulation& simulation) override; - - /** - * @brief Check whether the command was delivered. - * - * @return True once the command packet was queued, or if there was none. - */ - bool command_delivered() const { return this->command_sent; } - - /** - * @brief Get how many simulated milliseconds a press must be held. - * - * @note Button::update classifies on elapsed_time_ms strictly greater than - * each delay, so long and extra long presses are one millisecond past - * their thresholds. - * - * @param press Press to emit. - * @param delays Press durations the board classifies against. - * @return Hold time in simulated milliseconds, zero for no press. - */ - static uint32_t hold_time_ms(ButtonPress press, const ButtonDelays& delays); - -private: - /** - * @brief Script being played. - */ - ScenarioScript script; - - /** - * @brief Interface input written by the script. - * - * @note Bound for the life of the run; the context outlives every listener. - */ - ProxyState& state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Decoder used to address and queue the command. - * - * @note Bound for the life of the run; the context outlives every listener. - */ - Telemetry& telemetry; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief First tick the command may be injected on. - * - * @note Never tick 0: that tick carries the variable map request and the - * firmware drops the rest of its receive buffer after framing one - * packet. - */ - uint64_t command_tick; - - /** - * @brief Ticks the button is held between. - */ - ///@{ - uint64_t button_press_tick; - uint64_t button_release_tick; - ///@} - - /** - * @brief Whether the command still has to be injected. - */ - bool command_sent; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_CORE_SCENARIO_HPP diff --git a/include/micras/sim/core/serial_bus.hpp b/include/micras/sim/core/serial_bus.hpp deleted file mode 100644 index 4d08cbc..0000000 --- a/include/micras/sim/core/serial_bus.hpp +++ /dev/null @@ -1,134 +0,0 @@ -/** - * @file - * - * @brief In-process replacement for the bluetooth UART. - */ - -#ifndef MICRAS_SIM_CORE_SERIAL_BUS_HPP -#define MICRAS_SIM_CORE_SERIAL_BUS_HPP - -#include -#include -#include -#include - -namespace micras::sim { -/** - * @brief Receives every byte the firmware writes. - */ -class ISerialListener { -public: - ISerialListener() = default; - - ISerialListener(const ISerialListener&) = delete; - ISerialListener(ISerialListener&&) = delete; - ISerialListener& operator=(const ISerialListener&) = delete; - ISerialListener& operator=(ISerialListener&&) = delete; - - virtual ~ISerialListener() = default; - - /** - * @brief Handle bytes the firmware just wrote. - * - * @param bytes Bytes leaving the firmware, in stream order. - */ - virtual void on_firmware_bytes(std::span bytes) = 0; -}; - -/** - * @brief Byte channel between the firmware and everything listening to it. - * - * @note The firmware frames every complete packet it finds in one batch, so - * handing over one packet per call is not about what it can parse: it is - * what keeps a tick's input independent of how fast an asynchronous - * producer happens to be queueing. Output is fanned out to every listener - * as it is written, so no listener can consume bytes another one needed. - */ -class SerialBus { -public: - /** - * @brief Start delivering firmware output to a listener. - * - * @note Listeners must not be added or removed while bytes are being - * delivered. Adding the same listener twice is ignored. - * - * @param listener Listener to add; it must outlive the bus or remove itself. - */ - void add_listener(ISerialListener& listener); - - /** - * @brief Stop delivering firmware output to a listener. - * - * @param listener Listener to remove; unknown listeners are ignored. - */ - void remove_listener(const ISerialListener& listener); - - /** - * @brief Deliver bytes the firmware just wrote to every listener. - * - * @param bytes Bytes leaving the firmware. - */ - void send_from_firmware(std::span bytes); - - /** - * @brief Queue one complete packet for the firmware to read. - * - * @note The queue is bounded: a producer that outruns the one packet per - * tick the firmware is handed would otherwise grow it without limit - * and add unbounded latency. Packets past the bound are dropped and - * counted. - * - * @param packet Serialized packet, header to tail inclusive. - */ - void queue_for_firmware(std::span packet); - - /** - * @brief Take the next queued packet. - * - * @return The oldest queued packet, or an empty vector when none is waiting. - */ - std::vector take_for_firmware(); - - /** - * @brief Get how many packets are waiting for the firmware. - * - * @return Number of queued packets. - */ - std::size_t pending_packets() const { return this->to_firmware.size(); } - - /** - * @brief Get how many packets were dropped because the queue was full. - * - * @return Number of dropped packets, always 0 on a healthy run. - */ - uint32_t dropped_packets() const { return this->dropped; } - - /** - * @brief Largest number of packets allowed to wait for the firmware. - */ - static constexpr std::size_t max_pending_packets{256}; - - /** - * @brief Drop every queued packet. - */ - void clear(); - -private: - /** - * @brief Listeners receiving firmware output, in registration order. - */ - std::vector listeners; - - /** - * @brief Packets queued by the host, one handed to the firmware per call. - */ - std::deque> to_firmware; - - /** - * @brief Packets dropped because the queue was full. - */ - uint32_t dropped{0}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_CORE_SERIAL_BUS_HPP diff --git a/include/micras/sim/core/simulation_context.hpp b/include/micras/sim/core/simulation_context.hpp deleted file mode 100644 index 8230a23..0000000 --- a/include/micras/sim/core/simulation_context.hpp +++ /dev/null @@ -1,87 +0,0 @@ -/** - * @file - * - * @brief Everything the proxy layer sees of the simulation. - */ - -#ifndef MICRAS_SIM_CORE_SIMULATION_CONTEXT_HPP -#define MICRAS_SIM_CORE_SIMULATION_CONTEXT_HPP - -#include - -#include "micras/sim/core/clock.hpp" -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/core/proxy_state.hpp" -#include "micras/sim/core/serial_bus.hpp" - -namespace micras::sim { -class FirmwareThread; - -/** - * @brief Aggregate of the facets a proxy can reach, with no logic of its own. - * - * @note The firmware initialises its proxies from the const globals in - * config/target.hpp, before main runs, so the only route from a proxy to - * the simulation is something reachable from its Config. instance() is - * that route and is referenced by target.hpp alone: every proxy and every - * class under sim/ receives the facet it needs instead of calling it. - * Being a function-local static, it is constructed on the first call and - * therefore never after something that depends on it. - */ -struct SimulationContext { - /** - * @brief Get the process-wide context. - * - * @return Reference to the single instance. - */ - static SimulationContext& instance(); - - /** - * @brief MuJoCo model and data. - */ - MujocoWorld world; - - /** - * @brief Simulated time and tick arithmetic. - */ - Clock clock; - - /** - * @brief Byte channel to and from the firmware. - */ - SerialBus serial; - - /** - * @brief What crosses the proxy boundary. - */ - ProxyState proxy_state; - - /** - * @brief Program running in lockstep, or null when nothing is driving one. - * - * @note Set by the wiring entry point before the run starts. The Stopwatch - * reaches it to hand the tick over when the firmware waits for time - * to pass; without it a stopwatch simply reports the clock. - */ - std::atomic firmware{nullptr}; -}; - -/** - * @brief Dereference the context a proxy was configured with. - * - * @param context Context taken from the proxy Config. - * @return Reference to the context. - */ -SimulationContext& require_context(SimulationContext* context); - -/** - * @brief Hand the current tick over to the process-wide simulation. - * - * @note For the wiring that stands in for the MCU startup code, which has no - * Config to reach the context through. Does nothing when no program is - * being driven. - */ -void yield_tick(); -} // namespace micras::sim - -#endif // MICRAS_SIM_CORE_SIMULATION_CONTEXT_HPP diff --git a/include/micras/sim/recording/csv_recorder.hpp b/include/micras/sim/recording/csv_recorder.hpp deleted file mode 100644 index 64dc7a3..0000000 --- a/include/micras/sim/recording/csv_recorder.hpp +++ /dev/null @@ -1,155 +0,0 @@ -/** - * @file - * - * @brief One CSV row per firmware tick: ground truth followed by the firmware pool. - */ - -#ifndef MICRAS_SIM_RECORDING_CSV_RECORDER_HPP -#define MICRAS_SIM_RECORDING_CSV_RECORDER_HPP - -#include -#include -#include -#include - -#include "micras/sim/core/proxy_state.hpp" -#include "micras/sim/core/simulation.hpp" -#include "micras/sim/recording/csv_writer.hpp" -#include "micras/sim/recording/ground_truth.hpp" -#include "micras/sim/telemetry/telemetry.hpp" - -namespace micras::sim { -/** - * @brief Samples the simulation state once per firmware tick into a CSV file. - * - * @note The header is written on the first sample, once the firmware has - * answered the variable map request, so every pool column is known. - */ -class CsvRecorder : public IRunListener { -public: - /** - * @brief Open the CSV file and resolve every model id it needs. - * - * @note A program that does not serve the firmware variable pool, such as - * a hardware test, passes no decoder; the run then records the ground - * truth and the proxy boundary only. - * - * @param world Simulation world to sample. - * @param proxy_state Record of what crossed the proxy boundary. - * @param telemetry Decoder holding the firmware pool values, or null. - * @param path Path of the CSV file to write. - */ - CsvRecorder( - const MujocoWorld& world, const ProxyState& proxy_state, const Telemetry* telemetry, - const std::filesystem::path& path - ); - - /** - * @brief Write one CSV row for the current simulation state. - * - * @param tick Index of the firmware tick that was just executed. - */ - void sample(uint64_t tick); - - /** - * @brief Write the row of the tick that just ran. - * - * @param simulation Run that just advanced. - */ - void on_after_tick(const Simulation& simulation) override; - - /** - * @brief Get the number of firmware pool columns written. - * - * @return Number of pool columns. - */ - std::size_t pool_column_count() const { return this->pool_columns.size(); } - - /** - * @brief Get the robot body height sampled on the last row. - * - * @return Height of the robot body origin in meters. - */ - double last_z() const { return this->ground_truth.last_z(); } - - /** - * @brief Get the proxy boundary column names, in the order sample() fills them. - * - * @note The interface output is deliberately absent: this firmware build - * never lights the LED, sets an ARGB colour or plays a tone, so the - * columns would be constant zero. ProxyState still carries it, for - * the panel and for the day the firmware uses it. - * - * @return Column names, all prefixed to keep them apart from the pool ones. - */ - static std::vector proxy_columns(); - - /** - * @brief Turn a pool variable name into a CSV friendly column name. - * - * @param name Original variable name. - * @return Snake case name. - */ - static std::string column_name(const std::string& name); - -private: - /** - * @brief One CSV column fed by a firmware pool variable. - * - * @note Custom serializable types expand into several columns, so a - * variable may own more than one entry, one per serialized field. - */ - struct PoolColumn { - uint16_t id; - std::size_t component; - std::string name; - }; - - /** - * @brief Build the pool columns from the variable map and write the header. - */ - void write_header(); - - /** - * @brief Get the monitoring variables the run records, if any. - * - * @return The variables, empty when the program serves no pool. - */ - std::vector pool_variables() const; - - /** - * @brief Decoder holding the firmware pool values, or null when there is none. - */ - const Telemetry* telemetry; - - /** - * @brief Record of what crossed the proxy boundary. - * - * @note Bound for the life of the recorder; it is fed by the caller that - * also owns the run. - */ - const ProxyState& proxy_state; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Physical state sampler. - */ - GroundTruth ground_truth; - - /** - * @brief Output file. - */ - CsvWriter writer; - - /** - * @brief Pool columns written after the ground truth columns. - */ - std::vector pool_columns; - - /** - * @brief Whether the header was already written. - */ - bool header_written{false}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_RECORDING_CSV_RECORDER_HPP diff --git a/include/micras/sim/recording/ground_truth.hpp b/include/micras/sim/recording/ground_truth.hpp deleted file mode 100644 index f9b9cd1..0000000 --- a/include/micras/sim/recording/ground_truth.hpp +++ /dev/null @@ -1,123 +0,0 @@ -/** - * @file - * - * @brief Physical state of the robot read straight from MuJoCo. - */ - -#ifndef MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP -#define MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP - -#include -#include -#include - -#include "micras/sim/core/mujoco_world.hpp" -#include "micras/sim/recording/csv_writer.hpp" - -namespace micras::sim { -/** - * @brief Resolves the robot ids once and samples the ground truth columns every tick. - * - * @note MuJoCo free joints split their six velocity dofs across two frames: - * the three linear dofs are expressed in the world frame, the three - * angular dofs in the body frame. The columns are named accordingly - * (vx_world, vy_world, vz_world, wz_body) and v_forward projects the - * world linear velocity onto the body forward axis, which is body +y. - * This is the one class that indexes the MuJoCo C arrays directly. - */ -class GroundTruth { -public: - /** - * @brief Resolve every model id the columns need. - * - * @param world Loaded simulation world. - */ - explicit GroundTruth(const MujocoWorld& world); - - /** - * @brief Get the column names, in the order sample() fills them. - * - * @return Column names. - */ - static std::vector columns(); - - /** - * @brief Sample the current physical state. - * - * @param tick Index of the firmware tick that was just executed. - * @return One cell per column. - */ - std::vector sample(uint64_t tick); - - /** - * @brief Sum every MuJoCo warning counter raised so far. - * - * @param world Loaded simulation world. - * @return Total number of warnings. - */ - static int64_t warnings_total(const MujocoWorld& world); - - /** - * @brief Get the robot body height sampled on the last row. - * - * @return Height of the robot body origin in meters. - */ - double last_z() const { return this->last_body_z; } - -private: - /** - * @brief Per-geom contact statistics for a single step. - */ - struct ContactStats { - int count{0}; - double normal_force{0.0}; - double slip{0.0}; - - /** - * @brief Deepest penetration over the geom's contacts, NaN when it has none. - */ - double penetration{std::numeric_limits::quiet_NaN()}; - }; - - /** - * @brief Collect the contact statistics of one geom. - * - * @param geom_id Id of the geom. - * @return Aggregated statistics. - */ - ContactStats collect_contacts(int geom_id) const; - - /** - * @brief Simulation world being sampled. - * - * @note Bound for the life of the recorder; it is fed by the caller that - * also owns the run. - */ - const MujocoWorld& world; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Ids resolved once from the model. - */ - ///@{ - int body_id; - int left_wheel_geom; - int right_wheel_geom; - int caster_geom; - int base_geom; - int left_actuator; - int right_actuator; - int free_joint_qvel{}; - int left_wheel_qvel{}; - int right_wheel_qvel{}; - int left_wheel_qpos{}; - int right_wheel_qpos{}; - ///@} - - /** - * @brief Height of the robot body origin on the last sampled row. - */ - double last_body_z{0.0}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_RECORDING_GROUND_TRUTH_HPP diff --git a/include/micras/sim/telemetry/packet_framer.hpp b/include/micras/sim/telemetry/packet_framer.hpp deleted file mode 100644 index d6eb8ec..0000000 --- a/include/micras/sim/telemetry/packet_framer.hpp +++ /dev/null @@ -1,88 +0,0 @@ -/** - * @file - * - * @brief Splits the firmware byte stream into serialized packets. - */ - -#ifndef MICRAS_SIM_TELEMETRY_PACKET_FRAMER_HPP -#define MICRAS_SIM_TELEMETRY_PACKET_FRAMER_HPP - -#include -#include -#include - -namespace micras::sim { -/** - * @brief Accumulates raw bytes and yields complete comm::Packet frames. - * - * @note Packet::serialize only escapes the payload: the five bytes after the - * header (message type, id, payload size) are written raw and may hold - * an escape or tail value, so the escape-aware walk starts at the payload. - * The checksum byte is never a control byte, so the first unescaped tail - * from the payload on ends the packet. - */ -class PacketFramer { -public: - /** - * @brief Largest amount of unframed bytes kept before the stream is considered desynchronised. - * - * @note A packet carries at most a 65535 byte payload, which escapes to - * 131070 bytes, so the limit sits above the longest frame the - * firmware can emit and only a stream with no closing frame at all - * reaches it. - */ - static constexpr std::size_t max_buffer_size{128UL * 1024UL + 16UL}; - - /** - * @brief Append bytes received from the firmware. - * - * @param bytes Raw bytes, in stream order. - */ - void push(std::span bytes); - - /** - * @brief Extract every complete packet accumulated so far. - * - * @return Serialized packets, header to tail inclusive, in stream order. - */ - std::vector> drain(); - - /** - * @brief Get how many times the buffer overflowed and was truncated. - * - * @return Number of resynchronisations, always 0 on a healthy stream. - */ - uint32_t resync_count() const { return this->resyncs; } - -private: - /** - * @brief Find the end of the packet starting at a header position. - * - * @param start Index of the header byte. - * @return Index of the tail byte, or the buffer size if the packet is incomplete. - */ - std::size_t find_tail(std::size_t start) const; - - /** - * @brief Drop the whole buffer when no frame closes within the limit. - * - * @note Once that much has arrived without a single closing frame, nothing - * buffered can still become a packet, and keeping part of it would - * only let the scan latch onto a payload byte and swallow the next - * real packet. Framing restarts clean from the following byte. - */ - void resync_if_overflowed(); - - /** - * @brief Bytes received but not yet framed. - */ - std::vector buffer; - - /** - * @brief Number of times the buffer overflowed and was truncated. - */ - uint32_t resyncs{0}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_TELEMETRY_PACKET_FRAMER_HPP diff --git a/include/micras/sim/telemetry/telemetry.hpp b/include/micras/sim/telemetry/telemetry.hpp deleted file mode 100644 index 3cdcd53..0000000 --- a/include/micras/sim/telemetry/telemetry.hpp +++ /dev/null @@ -1,298 +0,0 @@ -/** - * @file - * - * @brief Decoder for the firmware serial protocol over the in-process channel. - */ - -#ifndef MICRAS_SIM_TELEMETRY_TELEMETRY_HPP -#define MICRAS_SIM_TELEMETRY_TELEMETRY_HPP - -#include -#include -#include -#include -#include - -#include "micras/sim/core/serial_bus.hpp" -#include "micras/sim/core/simulation.hpp" -#include "micras/sim/telemetry/packet_framer.hpp" - -namespace micras::sim { -/** - * @brief One entry of the firmware serial variable pool. - */ -struct PoolVariable { - uint16_t id; - std::string name; - std::string type; - bool read_only; -}; - -/** - * @brief Variable map reported by the firmware, indexed by id and by name. - */ -class VariableMap { -public: - /** - * @brief Parse a variable map response payload. - * - * @param payload Unescaped payload of the map response. - * @return The decoded map, or nullopt if the payload is truncated. - */ - static std::optional parse(const std::vector& payload); - - /** - * @brief Get the column suffixes a custom serializable type expands into. - * - * @note The firmware reports types through core::type_name, so they arrive - * fully qualified ("micras::nav::GridPose"); only the last component - * is matched. Scalars expand into a single unnamed column. - * - * @param type Type name as reported in the variable map response. - * @return Suffixes in serialization order, empty for scalar types. - */ - static std::vector type_column_suffixes(const std::string& type); - - /** - * @brief Get the read-only pool variables, ordered by id. - * - * @return Vector of monitoring variables. - */ - const std::vector& monitoring_variables() const { return this->monitoring; } - - /** - * @brief Look up a pool variable id by its firmware name. - * - * @param name Name as reported in the variable map response. - * @return Id of the variable, or nullopt if the pool has no such name. - */ - std::optional find_id(const std::string& name) const; - - /** - * @brief Get the type name of a variable. - * - * @param id Id of the variable in the pool. - * @return Type name, or an empty string for unknown ids. - */ - std::string type_of(uint16_t id) const; - -private: - /** - * @brief Read-only variables, ordered by id. - */ - std::vector monitoring; - - /** - * @brief Type of every variable, by id. - */ - std::unordered_map types; - - /** - * @brief Id of every variable, by name. - */ - std::unordered_map ids_by_name; -}; - -/** - * @brief Latest decoded value of every pool variable, one entry per serialized field. - */ -class PoolValues { -public: - /** - * @brief Decode a payload according to the type reported in the variable map. - * - * @note The custom serializable layouts mirror nav::GridPose::serialize - * (three bytes: x, y, orientation as Side, RIGHT=0 UP=1 LEFT=2 DOWN=3) - * and nav::State::serialize (five little-endian floats: x, y, - * orientation, linear velocity, angular velocity). Unknown types - * decode to NaN rather than to a plausible looking wrong number. - * - * @param type Type name of the variable. - * @param payload Little-endian serialized value. - * @return One value per column of the type. - */ - static std::vector decode(const std::string& type, const std::vector& payload); - - /** - * @brief Store the decoded value of a variable. - * - * @param id Id of the variable in the pool. - * @param type Type name of the variable. - * @param payload Little-endian serialized value. - */ - void store(uint16_t id, const std::string& type, const std::vector& payload); - - /** - * @brief Get the latest value decoded for a variable. - * - * @param id Id of the variable in the pool. - * @param component Index of the field for custom serializable types. - * @return Latest value, or NaN if nothing was decoded yet. - */ - double get(uint16_t id, std::size_t component = 0) const; - -private: - /** - * @brief Latest value of each variable, by id. - */ - std::unordered_map> values; -}; - -/** - * @brief Drains the firmware output and keeps the latest value of every pool variable. - */ -class Telemetry : public ISerialListener, public IRunListener { -public: - /** - * @brief Listen to a serial bus. - * - * @param serial Bus shared with the firmware. - */ - explicit Telemetry(SerialBus& serial); - - Telemetry(const Telemetry&) = delete; - Telemetry(Telemetry&&) = delete; - Telemetry& operator=(const Telemetry&) = delete; - Telemetry& operator=(Telemetry&&) = delete; - - /** - * @brief Stop listening to the bus. - */ - ~Telemetry() override; - - /** - * @brief Buffer bytes the firmware wrote, to be decoded on the next poll. - * - * @param bytes Bytes leaving the firmware. - */ - void on_firmware_bytes(std::span bytes) override; - - /** - * @brief Queue a variable map request for the firmware. - * - * @note The firmware clears its whole receive deque after framing a single - * packet, so the caller must guarantee that at most one packet is - * queued per tick; nothing here enforces it. - */ - void request_variable_map(); - - /** - * @brief Queue a value for a bidirectional variable. - * - * @param id Id of the variable in the pool. - * @param payload Little-endian serialized value. - */ - void send_variable(uint16_t id, const std::vector& payload); - - /** - * @brief Decode every packet buffered since the last call. - */ - void poll(); - - /** - * @brief Decode what the firmware wrote during the tick. - * - * @note Registered before the recorder so every column it feeds is already - * up to date when the row is written. - * - * @param simulation Run that just advanced. - */ - void on_after_tick(const Simulation& simulation) override; - - /** - * @brief Check whether the variable map has been received. - * - * @return True once the firmware answered the map request. - */ - bool has_variable_map() const { return this->map_received; } - - /** - * @brief Get the variable map as last reported by the firmware. - * - * @return The variable map, empty before the response arrives. - */ - const VariableMap& variable_map() const { return this->map; } - - /** - * @brief Look up a pool variable id by its firmware name. - * - * @param name Name as reported in the variable map response. - * @return Id of the variable, or nullopt if the pool has no such name. - */ - std::optional find_variable_id(const std::string& name) const { return this->map.find_id(name); } - - /** - * @brief Get the latest value decoded for a variable. - * - * @param id Id of the variable in the pool. - * @param component Index of the field for custom serializable types. - * @return Latest value, or NaN if nothing was decoded yet. - */ - double value(uint16_t id, std::size_t component = 0) const { return this->values.get(id, component); } - - /** - * @brief Get the latest value of a scalar variable by name. - * - * @param name Firmware name of the variable. - * @return Latest value, or NaN when the map is not in yet. - */ - double value_of(const std::string& name) const; - - /** - * @brief Get how many map responses were dropped as malformed. - * - * @return Number of unparsable variable map responses. - */ - uint32_t map_failure_count() const { return this->map_failures; } - - /** - * @brief Get how many times the receive buffer had to be truncated. - * - * @return Number of resynchronisations, always 0 on a healthy stream. - */ - uint32_t resync_count() const { return this->framer.resync_count(); } - -private: - /** - * @brief Handle one framed packet. - * - * @param raw Serialized packet, header to tail inclusive. - */ - void consume(const std::vector& raw); - - /** - * @brief Bus the firmware writes to and reads from. - * - * @note Bound for the life of the recorder; it is fed by the caller that - * also owns the run. - */ - SerialBus& serial; // NOLINT(*-avoid-const-or-ref-data-members) - - /** - * @brief Splits the firmware stream into packets. - */ - PacketFramer framer; - - /** - * @brief Variable map as last reported by the firmware. - */ - VariableMap map; - - /** - * @brief Latest value of every variable. - */ - PoolValues values; - - /** - * @brief Flag set once the variable map response was decoded. - */ - bool map_received{false}; - - /** - * @brief Number of map responses that could not be parsed. - */ - uint32_t map_failures{0}; -}; -} // namespace micras::sim - -#endif // MICRAS_SIM_TELEMETRY_TELEMETRY_HPP diff --git a/justfile b/justfile deleted file mode 100644 index f431107..0000000 --- a/justfile +++ /dev/null @@ -1,176 +0,0 @@ -# Human entry point for the Micras simulation harness. Linux only. - -set shell := ["bash", "-euo", "pipefail", "-c"] -set positional-arguments := true - -preset := env("MICRAS_PRESET", "default") -build_dir := "build" / preset -bin := build_dir / "micras_simulation" -python := "python3" - -model := "models/robot_v2.xml" -legacy_model := "models/robot_legacy.xml" -pool_columns := "29" -# The current baseline, compared byte for byte; previous versions keep their columns -# checked as a prefix so every run ever recorded stays comparable. -baseline := "baseline/v2" -previous_baselines := "baseline/v1 baseline/v0" -check_runs := "idle explore_v2 explore_legacy" -hardware_tests := "test_battery test_imu test_locomotion test_odometry test_rotary_sensors test_wall_sensors test_calibrate_feed_forward" - -default: - @just --list - -# Configure the chosen CMake preset (default, debug, release). -configure: - cmake --preset {{preset}} - -[private] -configured: - @test -f {{build_dir}}/CMakeCache.txt || cmake --preset {{preset}} - -# Configure if needed, then build. -build: configured - cmake --build --preset {{preset}} - -# Unit tests through CTest. -test: build - ctest --preset {{preset}} - -# Run the main firmware program with arbitrary harness flags. -run *args: build - {{bin}} "$@" - -# Build every firmware hardware test executable. -build-tests: configured - cmake --build {{build_dir}} --target hardware_tests - -# Run one firmware hardware test, e.g. `just run-test test_imu --seconds 4 --out runs/imu`. -run-test name *args: build-tests - {{build_dir}}/hardware_tests/"$1" --model {{model}} "${@:2}" - -# 4 s with no command -> runs/idle -run-idle: build - {{bin}} --model {{model}} --seconds 4 --out runs/idle - -# 8 s explore on robot_v2 -> runs/explore_v2 -run-explore: build - {{bin}} --model {{model}} --command explore --seconds 8 --out runs/explore_v2 - -# 8 s explore on the pre-v2 chassis -> runs/explore_legacy -run-explore-legacy: build - {{bin}} --model {{legacy_model}} --command explore --seconds 8 --out runs/explore_legacy - -# Plots and report.json for one run directory. -analyze run: - {{python}} tools/analyze.py "$1" - -# Run with the bridge open so micras-monitor can connect on ws://localhost:8080. -serve *args: build - {{bin}} --model {{model}} --command explore --seconds 120 --out runs/serve --monitor "$@" - -# Watch a run in a window. Space pauses, right steps, tab cycles cameras, esc quits. -watch *args: build - {{bin}} --model {{model}} --command explore --seconds 30 --out runs/watch --viewer "$@" - -# 100 s explore recorded offscreen, then analysed. -video: build - mkdir -p runs/video - {{bin}} --model {{model}} --command explore --seconds 100 --out runs/explore_v2 \ - --video runs/video/explore_v2.mp4 --video-fps 30 --video-camera "side tracking" --video-size 1280x720 - {{python}} tools/analyze.py runs/explore_v2 - -# Config drift against the firmware submodule. -drift: - {{python}} tools/check_config_drift.py - -# Reformat the harness sources in place. -format: configured - cmake --build {{build_dir}} --target format - -# Fail if any harness source is not clang-format clean. -format-check: configured - cmake --build {{build_dir}} --target format-check - -# clang-tidy over the harness sources. -lint: build - cmake --build {{build_dir}} --target lint - -# Compare the check scenarios against every recorded baseline. -compare-baseline: - for run in {{check_runs}}; do \ - {{python}} tools/compare_run.py runs/$run {{baseline}}/$run; \ - for previous in {{previous_baselines}}; do \ - {{python}} tools/compare_run.py runs/$run $previous/$run --columns-subset; \ - done; \ - done - -# Record a new baseline version. Refuses to overwrite one, so an older version -# is never lost and the prefix checks it anchors keep working. -record-baseline: test drift run-idle run-explore run-explore-legacy - for run in {{check_runs}}; do \ - test ! -e {{baseline}}/$run/data.csv.gz \ - || { echo "{{baseline}}/$run already exists; bump the baseline version instead"; exit 1; }; \ - done - for run in {{check_runs}}; do \ - mkdir -p {{baseline}}/$run; \ - gzip -9 -c runs/$run/data.csv > {{baseline}}/$run/data.csv.gz; \ - cp runs/$run/meta.json {{baseline}}/$run/meta.json; \ - done - just compare-baseline - -# Prove the monitor bridge talks and changes nothing: a client asks for the variable -# map over the socket, and the bridged run must match the same run without it. -check-monitor: build - {{bin}} --model {{model}} --command explore --seconds 6 --out runs/monitor/plain > /dev/null - {{python}} tools/monitor_probe.py --listen-seconds 4 --expect-packets 50 --request-map & \ - probe=$!; \ - {{bin}} --model {{model}} --command explore --seconds 60 --out runs/monitor/bridged --monitor > /dev/null; \ - wait $probe - {{bin}} --model {{model}} --command explore --seconds 6 --out runs/monitor/quiet --monitor > /dev/null - {{python}} tools/compare_run.py runs/monitor/quiet runs/monitor/plain - -# Prove a window changes nothing: the same scenario headless and watched must agree. -# Skipped with a notice when there is no display, so a headless machine still passes. -check-viewer: build - if command -v xvfb-run > /dev/null; then \ - show="xvfb-run -a"; \ - elif [ -n "${DISPLAY:-}" ]; then \ - show=""; \ - else \ - echo "no display and no xvfb-run, skipping the viewer comparison"; \ - exit 0; \ - fi; \ - {{bin}} --model {{model}} --command explore --seconds 3 --button short --out runs/viewer/headless > /dev/null; \ - $show {{bin}} --model {{model}} --command explore --seconds 3 --button short --out runs/viewer/watched \ - --viewer --viewer-size 640x480 > /dev/null; \ - {{python}} tools/compare_run.py runs/viewer/watched runs/viewer/headless - -# Run every hardware test briefly, to catch a program that stops handing ticks back. -smoke-tests: build-tests - for name in {{hardware_tests}}; do \ - echo "== $name"; \ - {{build_dir}}/hardware_tests/$name --model {{model}} --seconds 1 --out runs/smoke/$name > /dev/null; \ - done - {{build_dir}}/hardware_tests/test_fan --model {{model}} --seconds 1 --button short --out runs/smoke/test_fan \ - > /dev/null - -# The gate: build, tests, drift, scenarios, analysis, assertions, baseline. -check: build test drift check-options smoke-tests check-viewer check-monitor run-idle run-explore run-explore-legacy - for run in {{check_runs}}; do {{python}} tools/analyze.py runs/$run; done - {{python}} tools/check_run.py --pool-columns {{pool_columns}} runs/idle runs/explore_v2 runs/explore_legacy - just compare-baseline - -# Configure every optional subsystem off, so the options stay buildable. -check-options: - cmake -S . -B {{build_dir}}-minimal -G Ninja -DCMAKE_BUILD_TYPE=RelWithDebInfo \ - -DMICRAS_VIEWER=OFF -DMICRAS_VIDEO=OFF -DMICRAS_BRIDGE=OFF -DMICRAS_TESTS=OFF > /dev/null - cmake --build {{build_dir}}-minimal > /dev/null - -# Remove the build directory of the chosen preset. -clean: - rm -rf {{build_dir}} - -# Remove every build directory. -clean-all: - rm -rf build diff --git a/models/experiments/a0_legacy_mass.xml b/models/experiments/a0_legacy_mass.xml deleted file mode 100644 index 39b2d4c..0000000 --- a/models/experiments/a0_legacy_mass.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/a1_wheel_sphere.xml b/models/experiments/a1_wheel_sphere.xml deleted file mode 100644 index dbe88ce..0000000 --- a/models/experiments/a1_wheel_sphere.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/a2_wheel_capsule.xml b/models/experiments/a2_wheel_capsule.xml deleted file mode 100644 index 71fde76..0000000 --- a/models/experiments/a2_wheel_capsule.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/a3_wheel_ellipsoid.xml b/models/experiments/a3_wheel_ellipsoid.xml deleted file mode 100644 index f36e11d..0000000 --- a/models/experiments/a3_wheel_ellipsoid.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/l1_spawn_z.xml b/models/experiments/l1_spawn_z.xml deleted file mode 100644 index a5ab0d5..0000000 --- a/models/experiments/l1_spawn_z.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/l2_caster_coplanar.xml b/models/experiments/l2_caster_coplanar.xml deleted file mode 100644 index 926a91b..0000000 --- a/models/experiments/l2_caster_coplanar.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/l3_spawn_caster.xml b/models/experiments/l3_spawn_caster.xml deleted file mode 100644 index deba7b3..0000000 --- a/models/experiments/l3_spawn_caster.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/legacy_solref002.xml b/models/experiments/legacy_solref002.xml deleted file mode 100644 index 0040c0d..0000000 --- a/models/experiments/legacy_solref002.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m1_solref010.xml b/models/experiments/m1_solref010.xml deleted file mode 100644 index b5c3305..0000000 --- a/models/experiments/m1_solref010.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m2_solref005.xml b/models/experiments/m2_solref005.xml deleted file mode 100644 index e4950a2..0000000 --- a/models/experiments/m2_solref005.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m3_solref002.xml b/models/experiments/m3_solref002.xml deleted file mode 100644 index ad1015a..0000000 --- a/models/experiments/m3_solref002.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m4_solimp.xml b/models/experiments/m4_solimp.xml deleted file mode 100644 index 5a16a65..0000000 --- a/models/experiments/m4_solimp.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m5_impratio10.xml b/models/experiments/m5_impratio10.xml deleted file mode 100644 index 990cd2a..0000000 --- a/models/experiments/m5_impratio10.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/m6_noslip3.xml b/models/experiments/m6_noslip3.xml deleted file mode 100644 index cb16754..0000000 --- a/models/experiments/m6_noslip3.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/maze.xml b/models/experiments/maze.xml deleted file mode 100644 index 35bca6f..0000000 --- a/models/experiments/maze.xml +++ /dev/null @@ -1,582 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/models/experiments/n1_condim3.xml b/models/experiments/n1_condim3.xml deleted file mode 100644 index c4e6375..0000000 --- a/models/experiments/n1_condim3.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n2_pyramidal.xml b/models/experiments/n2_pyramidal.xml deleted file mode 100644 index e6852ac..0000000 --- a/models/experiments/n2_pyramidal.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n3_implicit.xml b/models/experiments/n3_implicit.xml deleted file mode 100644 index b0ed49b..0000000 --- a/models/experiments/n3_implicit.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n4_armature.xml b/models/experiments/n4_armature.xml deleted file mode 100644 index 0c2d740..0000000 --- a/models/experiments/n4_armature.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n5_motor.xml b/models/experiments/n5_motor.xml deleted file mode 100644 index ec7482a..0000000 --- a/models/experiments/n5_motor.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n6_motor_armature.xml b/models/experiments/n6_motor_armature.xml deleted file mode 100644 index 45d15d3..0000000 --- a/models/experiments/n6_motor_armature.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/n7_nofric23.xml b/models/experiments/n7_nofric23.xml deleted file mode 100644 index eb7fd1a..0000000 --- a/models/experiments/n7_nofric23.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p1_roll1e4.xml b/models/experiments/p1_roll1e4.xml deleted file mode 100644 index b4658db..0000000 --- a/models/experiments/p1_roll1e4.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p2_roll1e4_tors1e3.xml b/models/experiments/p2_roll1e4_tors1e3.xml deleted file mode 100644 index 8651e15..0000000 --- a/models/experiments/p2_roll1e4_tors1e3.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p3_condim3_motor.xml b/models/experiments/p3_condim3_motor.xml deleted file mode 100644 index d3ed0cd..0000000 --- a/models/experiments/p3_condim3_motor.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p4_roll_tors_motor.xml b/models/experiments/p4_roll_tors_motor.xml deleted file mode 100644 index 0e343e6..0000000 --- a/models/experiments/p4_roll_tors_motor.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p5_roll_tors_motor_arm.xml b/models/experiments/p5_roll_tors_motor_arm.xml deleted file mode 100644 index 6dd4a7e..0000000 --- a/models/experiments/p5_roll_tors_motor_arm.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p6_roll5e5.xml b/models/experiments/p6_roll5e5.xml deleted file mode 100644 index 05ef05f..0000000 --- a/models/experiments/p6_roll5e5.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/p7_nomassattr.xml b/models/experiments/p7_nomassattr.xml deleted file mode 100644 index b263c48..0000000 --- a/models/experiments/p7_nomassattr.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q10_noslip3.xml b/models/experiments/q10_noslip3.xml deleted file mode 100644 index 77994b7..0000000 --- a/models/experiments/q10_noslip3.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q11_nofrictionloss.xml b/models/experiments/q11_nofrictionloss.xml deleted file mode 100644 index b5ded27..0000000 --- a/models/experiments/q11_nofrictionloss.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q1_no_caster_fix.xml b/models/experiments/q1_no_caster_fix.xml deleted file mode 100644 index ea85a6e..0000000 --- a/models/experiments/q1_no_caster_fix.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q2_no_spawn_fix.xml b/models/experiments/q2_no_spawn_fix.xml deleted file mode 100644 index f766b91..0000000 --- a/models/experiments/q2_no_spawn_fix.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q3_no_both.xml b/models/experiments/q3_no_both.xml deleted file mode 100644 index 74f6db9..0000000 --- a/models/experiments/q3_no_both.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q4_solref005.xml b/models/experiments/q4_solref005.xml deleted file mode 100644 index 76fd535..0000000 --- a/models/experiments/q4_solref005.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q5_solref002.xml b/models/experiments/q5_solref002.xml deleted file mode 100644 index 96e31d0..0000000 --- a/models/experiments/q5_solref002.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q6_solimp.xml b/models/experiments/q6_solimp.xml deleted file mode 100644 index 1be3844..0000000 --- a/models/experiments/q6_solimp.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q7_sphere.xml b/models/experiments/q7_sphere.xml deleted file mode 100644 index 3d05833..0000000 --- a/models/experiments/q7_sphere.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q8_ellipsoid.xml b/models/experiments/q8_ellipsoid.xml deleted file mode 100644 index 4b9949f..0000000 --- a/models/experiments/q8_ellipsoid.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/q9_implicit.xml b/models/experiments/q9_implicit.xml deleted file mode 100644 index 4d6f829..0000000 --- a/models/experiments/q9_implicit.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/experiments/r1_caster_condim1.xml b/models/experiments/r1_caster_condim1.xml deleted file mode 100644 index cf721b1..0000000 --- a/models/experiments/r1_caster_condim1.xml +++ /dev/null @@ -1,132 +0,0 @@ - - - diff --git a/models/experiments/r2_caster_solref01.xml b/models/experiments/r2_caster_solref01.xml deleted file mode 100644 index b8a4893..0000000 --- a/models/experiments/r2_caster_solref01.xml +++ /dev/null @@ -1,132 +0,0 @@ - - - diff --git a/models/experiments/r3_caster_r006.xml b/models/experiments/r3_caster_r006.xml deleted file mode 100644 index 3b93122..0000000 --- a/models/experiments/r3_caster_r006.xml +++ /dev/null @@ -1,132 +0,0 @@ - - - diff --git a/models/gen_maze.py b/models/gen_maze.py deleted file mode 100755 index cd73827..0000000 --- a/models/gen_maze.py +++ /dev/null @@ -1,276 +0,0 @@ -#!/usr/bin/env python3 - -import sys -import argparse -from enum import StrEnum -from pathlib import Path - -CELL_CHAR_WIDTH = 4 -CELL_CHAR_HEIGHT = 2 - -class MazeSymbol(StrEnum): - POST = 'o' - WALL_HORIZONTAL = '---' - WALL_HORIZONTAL_EMPTY = ' ' - WALL_VERTICAL = '|' - WALL_VERTICAL_EMPTY = '' - START = 'S' - GOAL = 'G' - -class MazeParser: - def __init__(self, cell_size: float = 0.18, wall_thickness: float = 0.012, wall_height: float = 0.05): - self.cell_size = cell_size - self.wall_thickness = wall_thickness - self.wall_height = wall_height - self.width = 0 - self.height = 0 - self.start = None - self.goals = [] - - def parse_maze_file(self, maze_file: str) -> list[str]: - with open(maze_file, 'r') as f: - lines = [line.rstrip() for line in f.readlines()] - - if not lines: - raise ValueError("Empty maze file") - - self.maze_width = (len(lines[0]) - 1) // CELL_CHAR_WIDTH - self.maze_height = (len(lines) - 1) // CELL_CHAR_HEIGHT - - print(f"Detected maze dimensions: {self.maze_width}x{self.maze_height}") - return lines - - def extract_horizontal_walls(self, lines: list[str]) -> list[list[bool]]: - horizontal_walls = [] - for i in range(0, len(lines), 2): - row = [] - for j in range(0, len(lines[i]), 4): - if lines[i][j + 1:j + 4] == MazeSymbol.WALL_HORIZONTAL: - row.append(True) - else: - row.append(False) - horizontal_walls.append(row) - return horizontal_walls - - - def extract_vertical_walls(self, lines: list[str]) -> list[list[bool]]: - vertical_walls = [] - for i in range(1, len(lines), 2): - row = [] - for j in range(0, len(lines[i]), 4): - if lines[i][j] == MazeSymbol.WALL_VERTICAL: - row.append(True) - else: - row.append(False) - vertical_walls.append(row) - return vertical_walls - - def find_start_and_goals(self, lines: list[str]) -> tuple[tuple[int, int], list[tuple[int, int]]]: - start = None - goals = [] - - row = 0 - col = 0 - - inverted_lines = lines[::-1] - - for i in range(1, len(inverted_lines), 2): - for j in range(2, len(inverted_lines[i]), 4): - if inverted_lines[i][j] == MazeSymbol.START: - if start is not None: - raise ValueError("Multiple start points found") - start = (row, col) - elif inverted_lines[i][j] == MazeSymbol.GOAL: - goals.append((row, col)) - col += 1 - row += 1 - col = 0 - - self.start = start - self.goals = goals - - return start, goals - - def is_start_or_goal(self, row: int, col: int) -> bool: - if (row, col) == self.start: - return True - if (row, col) in self.goals: - return True - return False - - def is_post_on_start_cell(self, row: int, col: int) -> bool: - if self.is_start_or_goal(row, col) or \ - self.is_start_or_goal(row - 1, col) or \ - self.is_start_or_goal(row, col - 1) or \ - self.is_start_or_goal(row - 1, col - 1): - return True - - return False - - def is_horizontal_wall_on_start_cell(self, row: int, col: int) -> bool: - if self.is_start_or_goal(row - 1, col) or \ - self.is_start_or_goal(row, col): - return True - - return False - - def is_vertical_wall_on_start_cell(self, row: int, col: int) -> bool: - if self.is_start_or_goal(row, col) or \ - self.is_start_or_goal(row, col - 1): - return True - - return False - - def generate_posts_xml(self) -> list[str]: - xml = [] - - for row in range(self.maze_height + 1): - for col in range(self.maze_width + 1): - x = col * self.cell_size - y = row * self.cell_size - - class_name = 'post_w' if self.is_post_on_start_cell(row, col) else 'post' - xml.append( - f' ' - ) - return xml - - def generate_horizontal_walls_xml(self, horizontal_walls: list[list[bool]]) -> list[str]: - xml = [] - inverted_horizontal_walls = horizontal_walls[::-1] - - for row in range(len(inverted_horizontal_walls)): - for col in range(len(inverted_horizontal_walls[row])): - if inverted_horizontal_walls[row][col]: - x = col * self.cell_size + self.cell_size / 2 - y = row * self.cell_size - - class_name = 'hwall_w' if self.is_horizontal_wall_on_start_cell(row, col) else 'hwall' - xml.append( - f' ' - ) - return xml - - def generate_vertical_walls_xml(self, vertical_walls: list[list[bool]]) -> list[str]: - xml = [] - inverted_vertical_walls = vertical_walls[::-1] - - for row in range(len(inverted_vertical_walls)): - for col in range(len(inverted_vertical_walls[row])): - if inverted_vertical_walls[row][col]: - x = col * self.cell_size - y = row * self.cell_size + self.cell_size / 2 - - class_name = 'vwall_w' if self.is_vertical_wall_on_start_cell(row, col) else 'vwall' - xml.append( - f' ' - ) - return xml - - def add_header(self) -> str: - xml = [] - xml.append('') - xml.append(f'') - xml.append(f'\n') - return '\n'.join(xml) - - def generate_assets(self) -> str: - xml = [] - xml.append(' ') - xml.append(' ') - xml.append(' ') - xml.append(' \n') - return '\n'.join(xml) - - def generate_default_class(self) -> str: - xml = [] - red_wall_parameters = 'type="box" zaxis="0 1 0" material="walls"' - white_wall_parameters = 'type="box" zaxis="0 1 0" rgba="1 1 1 1"' - length = (self.cell_size - self.wall_thickness) / 2 - height = self.wall_height / 2 - width = self.wall_thickness / 2 - - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' ') - xml.append(f' ') - xml.append(' ') - xml.append(' \n') - return '\n'.join(xml) - - def generate_top_camera_view(self) -> str: - xml = [] - maze_center = (self.maze_width * self.cell_size / 2, self.maze_height * self.cell_size / 2) - xml.append(f' ') - return '\n'.join(xml) - - def generate_xml(self, horizontal_walls: list[list[bool]], vertical_walls: list[list[bool]]) -> str: - xml_lines = [] - xml_lines.append(self.add_header()) - xml_lines.append('') - xml_lines.append(self.generate_assets()) - xml_lines.append(self.generate_default_class()) - xml_lines.append(' ') - xml_lines.append(self.generate_top_camera_view()) - xml_lines.extend(self.generate_posts_xml()) - xml_lines.extend(self.generate_horizontal_walls_xml(horizontal_walls)) - xml_lines.extend(self.generate_vertical_walls_xml(vertical_walls)) - xml_lines.append(' ') - xml_lines.append('') - return '\n'.join(xml_lines) - -def main(): - parser = argparse.ArgumentParser(description="Generate MuJoCo XML maze walls from text maze file") - parser.add_argument("maze_file", help="Path to the maze text file") - parser.add_argument("-o", "--output", help="Output XML file (default: maze.xml)") - parser.add_argument("--cell-size", type=float, default=0.18, help="Cell size in meters (default: 0.18)") - parser.add_argument("--wall-thickness", type=float, default=0.0126, help="Wall thickness in meters (default: 0.0126)") - parser.add_argument("--wall-height", type=float, default=0.05, help="Wall height in meters (default: 0.05)") - - args = parser.parse_args() - - if not Path(args.maze_file).exists(): - print(f"Error: Maze file '{args.maze_file}' not found") - sys.exit(1) - - maze_file_path = Path(args.maze_file) - output_file = args.output or (maze_file_path.parent / "maze.xml") - - try: - maze_parser = MazeParser(args.cell_size, args.wall_thickness) - lines = maze_parser.parse_maze_file(args.maze_file) - horizontal_walls = maze_parser.extract_horizontal_walls(lines) - vertical_walls = maze_parser.extract_vertical_walls(lines) - start, goals = maze_parser.find_start_and_goals(lines) - print(f"Start point: {start}, Goals: {goals}") - xml_content = maze_parser.generate_xml(horizontal_walls, vertical_walls) - - with open(output_file, 'w') as f: - f.write(xml_content) - - print(f"Generated XML maze walls saved to '{output_file}'") - - except ValueError as e: - print(f"Error parsing maze file: {e}") - sys.exit(1) - - -if __name__ == "__main__": - main() diff --git a/models/maze.xml b/models/maze.xml deleted file mode 100644 index 35bca6f..0000000 --- a/models/maze.xml +++ /dev/null @@ -1,582 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/models/maze2.xml b/models/maze2.xml deleted file mode 100644 index 3091419..0000000 --- a/models/maze2.xml +++ /dev/null @@ -1,601 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/models/robot_legacy.xml b/models/robot_legacy.xml deleted file mode 100644 index 4063fb5..0000000 --- a/models/robot_legacy.xml +++ /dev/null @@ -1,98 +0,0 @@ - - diff --git a/models/robot_v2.xml b/models/robot_v2.xml deleted file mode 100644 index b1c0655..0000000 --- a/models/robot_v2.xml +++ /dev/null @@ -1,150 +0,0 @@ - - - diff --git a/models/robot_v2_maze2.xml b/models/robot_v2_maze2.xml deleted file mode 100644 index c0e7e9d..0000000 --- a/models/robot_v2_maze2.xml +++ /dev/null @@ -1,150 +0,0 @@ - - - diff --git a/scripts/check_baseline.sh b/scripts/check_baseline.sh new file mode 100755 index 0000000..8207f28 --- /dev/null +++ b/scripts/check_baseline.sh @@ -0,0 +1,48 @@ +#!/bin/bash +# check_baseline.sh compare|record ... +# +# Runs each named scenario and compares its summary with / (tools/baseline.py), +# byte for byte when is ON, then checks the health of every run (tools/check_run.py). record +# writes the summaries instead. + +set -euo pipefail + +if [[ $# -lt 8 ]]; then + echo "usage: $0 compare|record ..." >&2 + exit 2 +fi + +action=$1 +binary=$2 +scenarios=$3 +baseline=$4 +tools=$5 +out=$6 +exact=$7 +shift 7 + +exact_flag=() + +if [[ "${exact}" == ON ]]; then + exact_flag=(--exact) +fi + +runs=() +mkdir -p "${out}" + +# Each run starts from the output directory with a copy of its scenario, so the paths a summary records +# are the same on every machine. +for name in "$@"; do + rm -rf "${out:?}/${name}" + cp "${scenarios}/${name}.toml" "${out}/${name}.toml" + (cd "${out}" && "${binary}" --scenario "${name}.toml" --out "${name}" > "${name}.log") + runs+=("${out}/${name}") + + if [[ "${action}" == record ]]; then + python3 "${tools}/baseline.py" record "${out}/${name}" "${baseline}/${name}" + else + python3 "${tools}/baseline.py" compare "${out}/${name}" "${baseline}/${name}" "${exact_flag[@]}" + fi +done + +python3 "${tools}/check_run.py" "${runs[@]}" diff --git a/scripts/check_generic.sh b/scripts/check_generic.sh new file mode 100755 index 0000000..35ca32e --- /dev/null +++ b/scripts/check_generic.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# check_generic.sh +# +# Fails if anything outside targets/ names a specific robot: every file of the repository is scanned, +# documentation and build files included, except the files micras-lib shares with every repository +# (they name the repositories that carry them) and this script, which lists the names. Of the +# Dockerfile, only the shared host stage is left out. In a git checkout the files are the tracked and +# the untracked but not ignored ones; elsewhere, every file outside build/. + +set -euo pipefail + +if [[ $# -ne 1 ]]; then + echo "usage: $0 " >&2 + exit 2 +fi + +cd "$1" + +identifiers='micras::Micras|MicrasTarget|SimulationContext|ProxyState|proxy_state|robot_v2|"micras"|FSM State|Desired Linear|Desired Angular|Odometry Linear|Odometry Angular|Grid Pose|grid_pose|fsm_state|odometry_state|front wheel|MicrasFirmware|MicrasBoard|micras_firmware|targets/micras|wall_adc|dip_switch|button_config|loop_time_us\{1042|hadc[0-9]|htim[0-9]|hspi[0-9]|huart[0-9]|MX_[A-Z0-9]+_Init|lsm6dsv|as5047' + +shared=( + .clang-format + .clang-tidy + tests/.clang-tidy + cmake/micras_warnings.cmake + cmake/templates/run_clang_tidy.sh.in + .docker/Dockerfile + scripts/check_generic.sh +) + +files=() + +if git rev-parse --is-inside-work-tree > /dev/null 2>&1; then + while IFS= read -r -d '' file; do + files+=("${file}") + done < <(git ls-files -z --cached --others --exclude-standard) +else + while IFS= read -r -d '' file; do + files+=("${file#./}") + done < <(find . \( -path ./build -o -path ./.git -o -name __pycache__ \) -prune -o -type f -print0) +fi + +scanned=() + +for file in "${files[@]}"; do + if [[ "${file}" == targets/* || ! -f "${file}" ]]; then + continue + fi + + for skipped in "${shared[@]}"; do + if [[ "${file}" == "${skipped}" ]]; then + continue 2 + fi + done + + scanned+=("${file}") +done + +found=0 + +if [[ ${#scanned[@]} -gt 0 ]] && grep -nE -I "${identifiers}" "${scanned[@]}"; then + found=1 +fi + +if [[ -f .docker/Dockerfile ]] && + sed '/^# >>> host stage >>>$/,/^# <<< host stage <<<$/d' .docker/Dockerfile | grep -nE "${identifiers}"; then + found=1 +fi + +if [[ ${found} -ne 0 ]]; then + echo "robot identifiers outside targets/: move them into the robot's target" + exit 1 +fi + +echo "ok nothing outside targets/ names a robot (${#scanned[@]} files)" diff --git a/scripts/check_invariance.sh b/scripts/check_invariance.sh new file mode 100755 index 0000000..1189006 --- /dev/null +++ b/scripts/check_invariance.sh @@ -0,0 +1,65 @@ +#!/bin/bash +# check_invariance.sh +# +# Proves that nothing but the physics decides a run: the same scenario run twice, then with a window, +# with a video and with a bridge nobody connects to, each data.csv and meta.json the same as the plain +# run's (tools/compare_run.py). The window needs xvfb-run or a display; MICRAS_SKIP_VIEWER=1 skips it +# even where one exists. +# The video needs EGL, a GPU or Mesa's surfaceless platform. + +set -euo pipefail + +if [[ $# -ne 4 ]]; then + echo "usage: $0 " >&2 + exit 2 +fi + +binary=$1 +scenario=$2 +tools=$3 +out=$4 + +rm -rf "${out}" +mkdir -p "${out}" + +plain_run() { + local name=$1 + shift + "${binary}" --scenario "${scenario}" --out "${out}/${name}" "$@" > "${out}/${name}.log" +} + +compare() { + python3 "${tools}/compare_run.py" "${out}/$1" "${out}/plain" +} + +# The window and the video load the system's GL and font libraries, which keep what they cache until +# the process exits; a sanitized build still checks for leaks in every other run. +graphics_options="${ASAN_OPTIONS:+${ASAN_OPTIONS}:}detect_leaks=0" + +plain_run plain +plain_run again +compare again + +if [[ "${MICRAS_SKIP_VIEWER:-0}" == 1 ]]; then + echo "MICRAS_SKIP_VIEWER=1: skipping the viewer comparison" +elif command -v xvfb-run > /dev/null; then + ASAN_OPTIONS="${graphics_options}" xvfb-run -a "${binary}" --scenario "${scenario}" --out "${out}/viewer" --viewer --viewer-size 640x480 \ + > "${out}/viewer.log" + compare viewer +elif [[ -n "${DISPLAY:-}" ]]; then + ASAN_OPTIONS="${graphics_options}" "${binary}" --scenario "${scenario}" --out "${out}/viewer" --viewer --viewer-size 640x480 > "${out}/viewer.log" + compare viewer +else + echo "no display and no xvfb-run; install xvfb or set MICRAS_SKIP_VIEWER=1" >&2 + exit 1 +fi + +mkdir -p "${out}/video" +ASAN_OPTIONS="${graphics_options}" plain_run video --video "${out}/video/run.mp4" --video-size 640x480 +grep -q "^recording " "${out}/video.log" +compare video + +port=$(python3 -c 'import socket; s = socket.socket(); s.bind(("127.0.0.1", 0)); print(s.getsockname()[1])') +plain_run monitor --monitor --monitor-port "${port}" +grep -q "monitor bridge listening" "${out}/monitor.log" +compare monitor diff --git a/scripts/check_options.sh b/scripts/check_options.sh new file mode 100755 index 0000000..525a09d --- /dev/null +++ b/scripts/check_options.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# check_options.sh ... +# +# Configures and builds the simulator with every optional part off, the viewer, the video, the bridge, +# the tests and the targets, so that their stubs keep compiling. The remaining arguments go to CMake, +# for example the dependencies' sources the parent build already fetched. + +set -euo pipefail + +if [[ $# -lt 7 ]]; then + echo "usage: $0 ..." >&2 + exit 2 +fi + +source_dir=$1 +build_dir=$2 +generator=$3 +c_compiler=$4 +cxx_compiler=$5 +build_type=$6 +werror=$7 +shift 7 + +cmake -S "${source_dir}" -B "${build_dir}" -G "${generator}" \ + -DCMAKE_BUILD_TYPE="${build_type}" \ + -DCMAKE_C_COMPILER="${c_compiler}" \ + -DCMAKE_CXX_COMPILER="${cxx_compiler}" \ + -DMICRAS_SIM_VIEWER=OFF \ + -DMICRAS_SIM_VIDEO=OFF \ + -DMICRAS_SIM_BRIDGE=OFF \ + -DMICRAS_SIM_TESTS=OFF \ + -DMICRAS_SIM_TARGETS=OFF \ + -DMICRAS_SIM_WERROR="${werror}" \ + "$@" > "${build_dir}.log" + +cmake --build "${build_dir}" >> "${build_dir}.log" +echo "ok every option off configures and builds (${build_dir})" diff --git a/src/main.cpp b/src/main.cpp deleted file mode 100644 index 3bb1705..0000000 --- a/src/main.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/** - * @file - * - * @brief Runs the Micras firmware loop in the deterministic MuJoCo harness. - */ - -#include - -#include "micras/micras.hpp" -#include "micras/sim/app/application.hpp" - -int main(int argc, char** argv) { - return micras::sim::Application::main(std::span(argv, static_cast(argc)), [] { - micras::Micras micras; - - while (true) { - micras.update(); - } - }); -} diff --git a/src/proxy/argb.cpp b/src/proxy/argb.cpp deleted file mode 100644 index b75f056..0000000 --- a/src/proxy/argb.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/** - * @file - * - * @brief Addressable RGB stub recording the last colour written to each led. - */ - -#ifndef MICRAS_PROXY_ARGB_CPP -#define MICRAS_PROXY_ARGB_CPP - -#include "micras/proxy/argb.hpp" // NOLINT(misc-header-include-cycle) -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -template -TArgb::TArgb(const Config& config) : state{sim::require_context(config.context).proxy_state} { - this->turn_off(); -} - -template -void TArgb::publish() const { - static_assert( - num_of_leds <= sim::InterfaceOutput::argb_count, - "the board has more addressable LEDs than the simulated interface records" - ); - - for (uint8_t i = 0; i < num_of_leds; i++) { - const Color& colour = this->colors.at(i); - this->state.interface_output.argb.at(i) = {colour.red, colour.green, colour.blue}; - } -} - -template -void TArgb::set_color(const Color& color, uint8_t index) { - if (index >= num_of_leds) { - return; - } - - this->colors.at(index) = color; - this->publish(); -} - -template -void TArgb::set_color(const Color& color) { - this->colors.fill(color); - this->publish(); -} - -template -void TArgb::set_colors(const std::array& colors) { - this->colors = colors; - this->publish(); -} - -template -void TArgb::turn_off(uint8_t index) { - this->set_color({0, 0, 0}, index); -} - -template -void TArgb::turn_off() { - this->set_color({0, 0, 0}); -} - -template -void TArgb::update() { } - -template -typename TArgb::Color TArgb::get_color(uint8_t index) const { - return this->colors.at(index); -} -} // namespace micras::proxy - -#endif // MICRAS_PROXY_ARGB_CPP diff --git a/src/proxy/battery.cpp b/src/proxy/battery.cpp deleted file mode 100644 index 2fbdda2..0000000 --- a/src/proxy/battery.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/** - * @file - * - * @brief Battery proxy reporting a constant, noise free voltage. - */ - -#include "micras/proxy/battery.hpp" - -namespace micras::proxy { -Battery::Battery(const Config& config) : - reading{config.max_voltage}, max_voltage{config.max_voltage}, max_reading{config.max_reading} { } - -void Battery::update() { } - -float Battery::get_voltage() const { - return this->reading; -} - -float Battery::get_voltage_raw() const { - return (this->reading / this->max_voltage) * static_cast(this->max_reading); -} - -float Battery::get_adc_reading() const { - return this->reading / this->max_voltage; -} -} // namespace micras::proxy diff --git a/src/proxy/bluetooth_serial.cpp b/src/proxy/bluetooth_serial.cpp deleted file mode 100644 index 4d474b5..0000000 --- a/src/proxy/bluetooth_serial.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/** - * @file - * - * @brief Bluetooth serial proxy backed by the in-process byte queues of the world. - */ - -#include "micras/proxy/bluetooth_serial.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -BluetoothSerial::BluetoothSerial(const Config& config) : serial{sim::require_context(config.context).serial} { } - -void BluetoothSerial::update() { } - -void BluetoothSerial::send_data(std::vector data) { - this->serial.send_from_firmware(data); -} - -std::vector BluetoothSerial::get_data() { - return this->serial.take_for_firmware(); -} -} // namespace micras::proxy diff --git a/src/proxy/button.cpp b/src/proxy/button.cpp deleted file mode 100644 index 774b64a..0000000 --- a/src/proxy/button.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/** - * @file - * - * @brief Button proxy reading its pressed state from the simulated world. - */ - -#include "micras/proxy/button.hpp" -#include "micras/sim/core/firmware_thread.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Button::Button(const Config& config) : - simulation{sim::require_context(config.context)}, - last_update_us{this->simulation.clock.now_us()}, - long_press_delay{config.long_press_delay}, - extra_long_press_delay{config.extra_long_press_delay}, - status_stopwatch{Stopwatch::Config{.context = config.context}} { } - -bool Button::is_pressed() const { - return this->current_state; -} - -Button::Status Button::get_status() const { - return this->current_status; -} - -void Button::update() { - const uint64_t now = this->simulation.clock.now_us(); - this->repeated_updates = now == this->last_update_us ? this->repeated_updates + 1 : 1; - - sim::FirmwareThread* firmware = this->simulation.firmware.load(); - - if (this->repeated_updates >= max_repeated_updates and firmware != nullptr) { - firmware->yield_tick(); - this->repeated_updates = 0; - } - - this->last_update_us = this->simulation.clock.now_us(); - this->previous_state = this->current_state; - this->update_state(); - - if (this->is_rising_edge()) { - this->status_stopwatch.reset_ms(); - } else if (this->is_falling_edge()) { - if (this->status_stopwatch.elapsed_time_ms() > this->extra_long_press_delay) { - this->current_status = EXTRA_LONG_PRESS; - return; - } - - if (this->status_stopwatch.elapsed_time_ms() > this->long_press_delay) { - this->current_status = LONG_PRESS; - return; - } - - this->current_status = SHORT_PRESS; - return; - } - - this->current_status = NO_PRESS; -} - -void Button::update_state() { - this->current_state = this->simulation.proxy_state.interface_input.button_pressed; -} - -bool Button::is_rising_edge() const { - return this->current_state and not this->previous_state; -} - -bool Button::is_falling_edge() const { - return not this->current_state and this->previous_state; -} -} // namespace micras::proxy diff --git a/src/proxy/buzzer.cpp b/src/proxy/buzzer.cpp deleted file mode 100644 index 3c89fc1..0000000 --- a/src/proxy/buzzer.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/** - * @file - * - * @brief Buzzer stub recording the last requested tone. - */ - -#include "micras/proxy/buzzer.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Buzzer::Buzzer(const Config& config) : - state{sim::require_context(config.context).proxy_state}, stopwatch{Stopwatch::Config{.context = config.context}} { - this->stop(); -} - -void Buzzer::publish() const { - this->state.interface_output.buzzer_frequency = this->frequency; -} - -void Buzzer::play(uint32_t frequency, uint32_t duration) { - this->frequency = frequency; - this->is_playing = true; - this->duration = duration; - this->publish(); - - if (duration > 0) { - this->stopwatch.reset_ms(); - } -} - -void Buzzer::update() { - if (this->is_playing and this->duration > 0 and this->stopwatch.elapsed_time_ms() > this->duration) { - this->stop(); - } -} - -void Buzzer::wait(uint32_t /*interval*/) { - this->stop(); -} - -void Buzzer::stop() { - this->frequency = 0; - this->is_playing = false; - this->publish(); -} - -uint32_t Buzzer::get_frequency() const { - return this->frequency; -} -} // namespace micras::proxy diff --git a/src/proxy/dip_switch.cpp b/src/proxy/dip_switch.cpp deleted file mode 100644 index 36e04eb..0000000 --- a/src/proxy/dip_switch.cpp +++ /dev/null @@ -1,44 +0,0 @@ -/** - * @file - * - * @brief DIP switch proxy reading its state from the simulated world. - */ - -#ifndef MICRAS_PROXY_DIP_SWITCH_CPP -#define MICRAS_PROXY_DIP_SWITCH_CPP - -#include "micras/proxy/dip_switch.hpp" // NOLINT(misc-header-include-cycle) -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -template -TDipSwitch::TDipSwitch(const Config& config) : - state{sim::require_context(config.context).proxy_state} { } - -template -bool TDipSwitch::get_switch_state(uint8_t switch_index) const { - if (switch_index >= num_of_switches) { - return false; - } - - static_assert( - num_of_switches <= sim::InterfaceInput::dip_switch_count, - "the board has more DIP switches than the simulated interface records" - ); - - return this->state.interface_input.dip_switches.at(switch_index); -} - -template -uint8_t TDipSwitch::get_switches_value() const { - uint8_t switches_value = 0; - - for (uint8_t i = 0; i < num_of_switches; i++) { - switches_value |= static_cast(this->get_switch_state(i)) << i; - } - - return switches_value; -} -} // namespace micras::proxy - -#endif // MICRAS_PROXY_DIP_SWITCH_CPP diff --git a/src/proxy/fan.cpp b/src/proxy/fan.cpp deleted file mode 100644 index f1fc9d4..0000000 --- a/src/proxy/fan.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/** - * @file - * - * @brief Fan proxy mirroring the firmware speed ramp on a MuJoCo actuator. - */ - -#include "micras/core/utils.hpp" -#include "micras/proxy/fan.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Fan::Fan(const Config& config) : - world{sim::require_context(config.context).world}, - state{sim::require_context(config.context).proxy_state}, - actuator_id{this->world.require_id(mjOBJ_ACTUATOR, config.actuator)}, - max_acceleration{config.max_acceleration}, - acceleration_stopwatch{Stopwatch::Config{.context = config.context}} { - this->stop(); - this->enable(); -} - -void Fan::enable() { - this->enabled = true; - this->apply(); -} - -void Fan::disable() { - this->enabled = false; - this->apply(); -} - -void Fan::set_speed(float speed) { - this->update(); - this->target_speed = speed; -} - -float Fan::update() { - this->current_speed = core::move_towards( - this->current_speed, this->target_speed, this->acceleration_stopwatch.elapsed_time_ms() * this->max_acceleration - ); - - this->acceleration_stopwatch.reset_ms(); - - if (this->current_speed == 0.0F) { - this->stop(); - } else { - this->apply(); - } - - return this->current_speed; -} - -void Fan::stop() { - this->state.actuators.fan_speed = 0.0F; - this->world.set_control(this->actuator_id, 0.0); -} - -void Fan::apply() { - const bool active = this->enabled and this->state.overrides.fan_enabled; - const float speed = active ? this->current_speed : 0.0F; - - this->state.actuators.fan_speed = speed; - this->world.set_control(this->actuator_id, speed); -} -} // namespace micras::proxy diff --git a/src/proxy/imu.cpp b/src/proxy/imu.cpp deleted file mode 100644 index ddb911b..0000000 --- a/src/proxy/imu.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file - * - * @brief IMU proxy reading the MuJoCo gyroscope and accelerometer sensors. - */ - -#include "micras/proxy/imu.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Imu::Imu(const Config& config) : - state{sim::require_context(config.context).proxy_state}, - world{sim::require_context(config.context).world}, - gyro_address{this->world.sensor_address(config.gyro_sensor)}, - accelerometer_address{this->world.sensor_address(config.accelerometer_sensor)} { - this->update(); -} - -void Imu::update() { - for (std::size_t i = 0; i < sim::Sensors::axis_count; i++) { - const int offset = static_cast(i); - - this->angular_velocity.at(i) = static_cast(this->world.sensor_value(this->gyro_address + offset)); - this->linear_acceleration.at(i) = - static_cast(this->world.sensor_value(this->accelerometer_address + offset)); - } - - this->state.sensors.angular_velocity = this->angular_velocity; - this->state.sensors.linear_acceleration = this->linear_acceleration; -} - -float Imu::get_angular_velocity(Axis axis) const { - return this->angular_velocity.at(static_cast(axis)); -} - -float Imu::get_linear_acceleration(Axis axis) const { - return this->linear_acceleration.at(static_cast(axis)); -} - -void Imu::calibrate() { } - -// NOLINTNEXTLINE(readability-convert-member-functions-to-static): mirrors the firmware proxy API. -bool Imu::was_initialized() const { - return true; -} - -// NOLINTNEXTLINE(readability-convert-member-functions-to-static): mirrors the firmware proxy API. -bool Imu::check_whoami() { - return true; -} -} // namespace micras::proxy diff --git a/src/proxy/led.cpp b/src/proxy/led.cpp deleted file mode 100644 index 7c7691b..0000000 --- a/src/proxy/led.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/** - * @file - * - * @brief Led stub recording the last state written. - */ - -#include "micras/proxy/led.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Led::Led(const Config& config) : state{sim::require_context(config.context).proxy_state} { - this->publish(); -} - -void Led::publish() const { - this->state.interface_output.led_on = this->state_on; -} - -void Led::turn_on() { - this->state_on = true; - this->publish(); -} - -void Led::turn_off() { - this->state_on = false; - this->publish(); -} - -void Led::toggle() { - this->state_on = not this->state_on; - this->publish(); -} - -bool Led::is_on() const { - return this->state_on; -} -} // namespace micras::proxy diff --git a/src/proxy/locomotion.cpp b/src/proxy/locomotion.cpp deleted file mode 100644 index 51a0d7c..0000000 --- a/src/proxy/locomotion.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/** - * @file - * - * @brief Locomotion proxy driving the two MuJoCo motor actuators. - */ - -#include - -#include "micras/proxy/locomotion.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Locomotion::Locomotion(const Config& config) : - state{sim::require_context(config.context).proxy_state}, - left_motor{config.left_motor}, - right_motor{config.right_motor} { - this->stop(); - this->disable(); -} - -void Locomotion::enable() { - this->enabled = true; -} - -void Locomotion::disable() { - this->enabled = false; -} - -void Locomotion::set_wheel_command(float left_command, float right_command) { - const float left = this->enabled ? left_command : 0.0F; - const float right = this->enabled ? right_command : 0.0F; - - this->state.actuators.left_command = left; - this->state.actuators.right_command = right; - - this->left_motor.set_command(left); - this->right_motor.set_command(right); -} - -void Locomotion::set_command(float linear, float angular) { - float left_command = linear - angular; - float right_command = linear + angular; - - if (std::abs(left_command) > 100.0F) { - left_command *= 100.0F / std::abs(left_command); - right_command *= 100.0F / std::abs(left_command); - } - - if (std::abs(right_command) > 100.0F) { - left_command *= 100.0F / std::abs(right_command); - right_command *= 100.0F / std::abs(right_command); - } - - this->set_wheel_command(left_command, right_command); -} - -void Locomotion::stop() { - this->set_wheel_command(0.0F, 0.0F); -} -} // namespace micras::proxy diff --git a/src/proxy/motor.cpp b/src/proxy/motor.cpp deleted file mode 100644 index a8dcc14..0000000 --- a/src/proxy/motor.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/** - * @file - * - * @brief Motor proxy writing a clamped percentage command to a MuJoCo actuator. - */ - -#include - -#include "micras/proxy/motor.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -Motor::Motor(const Config& config) : - world{sim::require_context(config.context).world}, - actuator_id{this->world.require_id(mjOBJ_ACTUATOR, config.actuator)} { - this->set_command(0.0F); -} - -void Motor::set_command(float command) { - this->world.set_control(this->actuator_id, std::clamp(command, -100.0F, 100.0F)); -} -} // namespace micras::proxy diff --git a/src/proxy/rotary_sensor.cpp b/src/proxy/rotary_sensor.cpp deleted file mode 100644 index 04db0ca..0000000 --- a/src/proxy/rotary_sensor.cpp +++ /dev/null @@ -1,28 +0,0 @@ -/** - * @file - * - * @brief Rotary sensor proxy reading a MuJoCo wheel joint position. - */ - -#include - -#include "micras/proxy/rotary_sensor.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -RotarySensor::RotarySensor(const Config& config) : - state{sim::require_context(config.context).proxy_state}, - wheel{config.wheel}, - world{sim::require_context(config.context).world}, - sensor_address{this->world.sensor_address(config.sensor)} { - if (this->wheel >= sim::Sensors::wheel_count) { - throw std::out_of_range("rotary sensor wheel index is outside the wheels the boundary records"); - } -} - -float RotarySensor::get_position() const { - const auto position = static_cast(this->world.sensor_value(this->sensor_address)); - this->state.sensors.encoder_positions.at(this->wheel) = position; - return position; -} -} // namespace micras::proxy diff --git a/src/proxy/stopwatch.cpp b/src/proxy/stopwatch.cpp deleted file mode 100644 index 64e5789..0000000 --- a/src/proxy/stopwatch.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @file - */ - -#include "micras/proxy/stopwatch.hpp" -#include "micras/sim/core/firmware_thread.hpp" -#include "micras/sim/core/simulation_context.hpp" -#include "target.hpp" - -namespace micras::proxy { -Stopwatch::Stopwatch() : Stopwatch(stopwatch_config) { } - -Stopwatch::Stopwatch(const Config& config) : - simulation{sim::require_context(config.context)}, - counter{this->simulation.clock.now_us() - this->simulation.clock.us_per_tick()}, - last_read_us{this->simulation.clock.now_us()} { } - -void Stopwatch::reset_ms() { - this->reset_us(); -} - -void Stopwatch::reset_us() { - this->counter = this->simulation.clock.now_us(); -} - -uint32_t Stopwatch::elapsed_time_ms() const { - const uint64_t before = this->simulation.clock.now_us(); - this->repeated_ms_reads = before == this->last_read_us ? this->repeated_ms_reads + 1 : 1; - - if (this->repeated_ms_reads >= max_repeated_ms_reads) { - this->yield_tick(); - this->note_yield(); - } - - this->last_read_us = this->simulation.clock.now_us(); - return static_cast((this->last_read_us - this->counter) / 1000); -} - -uint32_t Stopwatch::elapsed_time_us() const { - const uint64_t before = this->simulation.clock.now_us(); - this->repeated_reads = before == this->last_read_us ? this->repeated_reads + 1 : 1; - - if (this->repeated_reads >= max_repeated_reads) { - this->yield_tick(); - this->note_yield(); - } - - this->last_read_us = this->simulation.clock.now_us(); - return static_cast(this->last_read_us - this->counter); -} - -void Stopwatch::sleep_ms(uint32_t time) { - Stopwatch(stopwatch_config).sleep_us(time * 1000); -} - -void Stopwatch::sleep_us(uint32_t time) const { - const uint64_t target = this->simulation.clock.now_us() + time; - - while (this->simulation.clock.now_us() < target and this->yield_tick()) { } -} - -bool Stopwatch::yield_tick() const { - sim::FirmwareThread* firmware = this->simulation.firmware.load(); - return firmware != nullptr and firmware->yield_tick(); -} - -void Stopwatch::note_yield() const { - this->repeated_reads = 0; - this->repeated_ms_reads = 0; -} -} // namespace micras::proxy diff --git a/src/proxy/storage.cpp b/src/proxy/storage.cpp deleted file mode 100644 index 87a2ea6..0000000 --- a/src/proxy/storage.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/** - * @file - * - * @brief Storage proxy keeping the serialized maze in memory, blank every run. - */ - -#include -#include - -#include "micras/proxy/storage.hpp" - -namespace micras::proxy { -std::unordered_map>& Storage::pages() { - static std::unordered_map> storage_pages; - return storage_pages; -} - -Storage::Storage(const Config& config) : start_page{config.start_page}, number_of_pages{config.number_of_pages} { - const auto page = Storage::pages().find(this->start_page); - - if (page == Storage::pages().end() or page->second.size() < sizeof(uint64_t)) { - return; - } - - uint64_t header{}; - std::memcpy(&header, page->second.data(), sizeof(header)); - - if (header >> 48 != start_symbol) { - return; - } - - const auto total_size = static_cast(header >> 32); - const auto num_primitives = static_cast(header >> 16); - const auto num_serializables = static_cast(header); - - this->buffer.assign(page->second.begin() + sizeof(header), page->second.begin() + sizeof(header) + 8L * total_size); - - this->primitives = deserialize_var_map(this->buffer, num_primitives); - this->serializables = deserialize_var_map(this->buffer, num_serializables); -} - -void Storage::create(const std::string& name, const core::ISerializable& data) { - this->serializables[name].ram_pointer = &data; -} - -void Storage::sync(const std::string& name, core::ISerializable& data) { - if (this->serializables.contains(name) and this->serializables.at(name).ram_pointer == nullptr) { - const auto& serializable = this->serializables.at(name); - data.deserialize(&this->buffer.at(serializable.buffer_address), serializable.size); - } - - this->create(name, data); -} - -void Storage::save() { - this->buffer.clear(); - - for (auto& [name, variable] : this->primitives) { - if (variable.ram_pointer == nullptr) { - continue; - } - - // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) - const auto* aux = reinterpret_cast(variable.ram_pointer); - variable.buffer_address = this->buffer.size(); - // NOLINTNEXTLINE(cppcoreguidelines-pro-bounds-pointer-arithmetic): raw bytes of a primitive variable. - this->buffer.insert(this->buffer.end(), aux, aux + variable.size); - } - - for (auto& [name, variable] : this->serializables) { - if (variable.ram_pointer == nullptr) { - continue; - } - - const std::vector aux = variable.ram_pointer->serialize(); - variable.buffer_address = this->buffer.size(); - variable.size = aux.size(); - this->buffer.insert(this->buffer.end(), aux.begin(), aux.end()); - } - - const auto serialized_serializables = serialize_var_map(this->serializables); - this->buffer.insert(this->buffer.begin(), serialized_serializables.begin(), serialized_serializables.end()); - - const auto serialized_primitives = serialize_var_map(this->primitives); - this->buffer.insert(this->buffer.begin(), serialized_primitives.begin(), serialized_primitives.end()); - - this->buffer.insert(this->buffer.end(), (8 - (this->buffer.size() % 8)) % 8, 0); - const auto total_size = static_cast(this->buffer.size() / 8); - - std::array header{}; - - header[0] = this->serializables.size(); - header[1] = this->serializables.size() >> 8; - header[2] = this->primitives.size(); - header[3] = this->primitives.size() >> 8; - - header[4] = total_size; - header[5] = total_size >> 8; - header[6] = start_symbol & 0xFF; - header[7] = start_symbol >> 8; - - std::vector page(header.begin(), header.end()); - page.insert(page.end(), this->buffer.begin(), this->buffer.end()); - Storage::pages()[this->start_page] = std::move(page); -} - -template -std::vector Storage::serialize_var_map(const std::unordered_map& variables) { - std::vector buffer; - - for (const auto& [name, variable] : variables) { - buffer.emplace_back(name.size()); - buffer.insert(buffer.end(), name.begin(), name.end()); - - buffer.emplace_back(variable.buffer_address); - buffer.emplace_back(variable.buffer_address >> 8); - - buffer.emplace_back(variable.size); - buffer.emplace_back(variable.size >> 8); - } - - return buffer; -} - -template -std::unordered_map Storage::deserialize_var_map(std::vector& buffer, uint16_t num_vars) { - std::unordered_map variables; - - uint16_t current_addr = 0; - - for (uint16_t decoded_vars = 0; decoded_vars < num_vars; decoded_vars++) { - const uint8_t var_name_len = buffer.at(current_addr); - - const std::string var_name(buffer.begin() + current_addr + 1, buffer.begin() + current_addr + 1 + var_name_len); - current_addr += var_name_len + 1; - - variables[var_name].buffer_address = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; - current_addr += 2; - - variables.at(var_name).size = buffer.at(current_addr) | buffer.at(current_addr + 1) << 8; - current_addr += 2; - } - - buffer.erase(buffer.begin(), buffer.begin() + current_addr); - return variables; -} -} // namespace micras::proxy diff --git a/src/proxy/torque_sensors.cpp b/src/proxy/torque_sensors.cpp deleted file mode 100644 index 265c0fb..0000000 --- a/src/proxy/torque_sensors.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file - * - * @brief Torque sensor stub always reporting zero torque and current. - */ - -#ifndef MICRAS_PROXY_TORQUE_SENSORS_CPP -#define MICRAS_PROXY_TORQUE_SENSORS_CPP - -#include "micras/proxy/torque_sensors.hpp" // NOLINT(misc-header-include-cycle) - -namespace micras::proxy { -template -TTorqueSensors::TTorqueSensors(const Config& /*config*/) { } - -template -void TTorqueSensors::calibrate() { } - -template -void TTorqueSensors::update() { } - -template -float TTorqueSensors::get_torque(uint8_t /*sensor_index*/) const { - return 0.0F; -} - -template -float TTorqueSensors::get_torque_raw(uint8_t /*sensor_index*/) const { - return 0.0F; -} - -template -float TTorqueSensors::get_current(uint8_t /*sensor_index*/) const { - return 0.0F; -} - -template -float TTorqueSensors::get_current_raw(uint8_t /*sensor_index*/) const { - return 0.0F; -} - -template -float TTorqueSensors::get_adc_reading(uint8_t /*sensor_index*/) const { - return 0.0F; -} -} // namespace micras::proxy - -#endif // MICRAS_PROXY_TORQUE_SENSORS_CPP diff --git a/src/proxy/wall_sensors.cpp b/src/proxy/wall_sensors.cpp deleted file mode 100644 index c0b8155..0000000 --- a/src/proxy/wall_sensors.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/** - * @file - * - * @brief Wall sensor proxy reading the MuJoCo rangefinder sensors. - */ - -#ifndef MICRAS_PROXY_WALL_SENSORS_CPP -#define MICRAS_PROXY_WALL_SENSORS_CPP - -#include - -#include "micras/core/utils.hpp" -#include "micras/proxy/wall_sensors.hpp" // NOLINT(misc-header-include-cycle) -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -template -TWallSensors::TWallSensors(const Config& config) : - state{sim::require_context(config.context).proxy_state}, - world{sim::require_context(config.context).world}, - max_sensor_reading{config.max_sensor_reading}, - filters{core::make_array(config.filter_cutoff, config.sampling_frequency)}, - base_readings{config.base_readings}, - uncertainty{config.uncertainty}, - constant{static_cast( - -std::pow(config.max_sensor_distance, 2) * - std::log(1.0F - config.min_sensor_reading / config.max_sensor_reading) - )} { - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->sensor_addresses.at(i) = this->world.sensor_address(config.sensors.at(i)); - } - - this->turn_off(); -} - -template -void TWallSensors::turn_on() { - this->leds_on = true; -} - -template -void TWallSensors::turn_off() { - this->leds_on = false; -} - -template -void TWallSensors::update() { - for (uint8_t i = 0; i < num_of_sensors; i++) { - this->filters.at(i).update(this->get_adc_reading(i)); - } -} - -template -bool TWallSensors::get_wall(uint8_t sensor_index, bool disturbed) const { - return this->filters.at(sensor_index).get_last() > - this->base_readings.at(sensor_index) * this->uncertainty * (disturbed ? 1.2F : 1.0F); -} - -template -float TWallSensors::get_reading(uint8_t sensor_index) const { - return this->filters.at(sensor_index).get_last(); -} - -template -float TWallSensors::get_adc_reading(uint8_t sensor_index) const { - const auto raw_reading = static_cast(this->world.sensor_value(this->sensor_addresses.at(sensor_index))); - - if (raw_reading < 0.0F) { - return this->record(sensor_index, 0.0F); - } - - if (raw_reading == 0.0F) { - return this->record(sensor_index, this->max_sensor_reading); - } - - const float intensity = 1.0F / (raw_reading * raw_reading); - - return this->record(sensor_index, this->max_sensor_reading * (1.0F - std::exp(-this->constant * intensity))); -} - -template -float TWallSensors::record(uint8_t sensor_index, float reading) const { - static_assert( - num_of_sensors <= sim::Sensors::wall_sensor_count, - "the board has more wall sensors than the simulated boundary records" - ); - - this->state.sensors.wall_adc_readings.at(sensor_index) = reading; - return reading; -} - -template -float TWallSensors::get_sensor_error(uint8_t sensor_index) const { - return this->get_reading(sensor_index) - this->base_readings.at(sensor_index); -} - -template -void TWallSensors::calibrate_sensor(uint8_t sensor_index) { - this->base_readings.at(sensor_index) = this->get_reading(sensor_index); -} -} // namespace micras::proxy - -#endif // MICRAS_PROXY_WALL_SENSORS_CPP diff --git a/src/sim/app/application.cpp b/src/sim/app/application.cpp deleted file mode 100644 index ea03a52..0000000 --- a/src/sim/app/application.cpp +++ /dev/null @@ -1,270 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "constants.hpp" -#include "micras/sim/app/application.hpp" -#include "micras/sim/app/crash_reporter.hpp" -#include "micras/sim/recording/run_metadata.hpp" -#include "target.hpp" - -namespace micras::sim { -namespace { -/** - * @brief Load the model and configure the clock the run needs. - * - * @param options Parsed command line. - * @param context Context to set up. - * @return The context, set up. - */ -SimulationContext& prepare(const CliOptions& options, SimulationContext& context) { - context.world.load(options.model); - context.clock = Clock::from_model(context.world.timestep(), loop_time_us); - context.world.reset(); - return context; -} - -/** - * @brief Get the press durations the board classifies against. - * - * @return The delays from the Button config. - */ -ButtonDelays board_delays() { - return { - .long_press = button_config.long_press_delay, - .extra_long_press = button_config.extra_long_press_delay, - }; -} - -/** - * @brief Get how many ticks pass between two frames at a given rate. - * - * @param fps Frames per second of simulated time. - * @param clock Clock the frame period is derived from. - * @return Number of ticks per frame, at least one. - */ -uint64_t ticks_per_frame(int fps, const Clock& clock) { - const double loop_period = clock.us_per_tick() * 1e-6; - return std::max(1, static_cast(std::llround(1.0 / (fps * loop_period)))); -} - -/** - * @brief Open the bridge to micras-monitor, reporting why if it cannot listen. - * - * @note A port that is already taken is a warning, not a failure: the run is - * worth having without a monitor watching it. - * - * @param options Parsed command line. - * @param serial Bus the monitor is bridged onto. - * @return The bridge, or nullptr when none was asked for or it could not listen. - */ -std::unique_ptr make_monitor(const CliOptions& options, SerialBus& serial) { - if (not options.monitor_enabled) { - return nullptr; - } - - std::string error; - auto bridge = std::make_unique(serial, options.monitor_port, error); - - if (not bridge->is_open()) { - std::cerr << "monitor disabled: " << error << '\n'; - return nullptr; - } - - std::cout << "monitor bridge listening on ws://localhost:" << options.monitor_port << '\n'; - return bridge; -} - -/** - * @brief Open the live window, reporting why if it cannot open. - * - * @param options Parsed command line. - * @param context Context whose world is drawn. - * @return The viewer, or nullptr when no window was asked for or possible. - */ -std::unique_ptr - make_viewer(const CliOptions& options, SimulationContext& context, const Telemetry& telemetry) { - if (not options.viewer_enabled) { - return nullptr; - } - - ViewerConfig config = options.viewer; - config.ticks_per_frame = ticks_per_frame(options.viewer_fps, context.clock); - config.us_per_tick = context.clock.us_per_tick(); - - std::string error; - auto viewer = MujocoViewer::create(context.world, context.proxy_state, telemetry, config, error); - - if (viewer == nullptr) { - std::cerr << "viewer disabled: " << error << '\n'; - return nullptr; - } - - std::cout << "window open at " << config.width << "x" << config.height << ", camera '" << config.camera - << "', one frame every " << config.ticks_per_frame << " ticks\n"; - - return viewer; -} - -/** - * @brief Bring up the offscreen recorder, reporting why if it cannot run. - * - * @note The offscreen framebuffer is sized from the model, which carries no - * , so it is widened before the render - * context is built. Nothing in mjModel::vis affects physics. - * - * @param options Parsed command line. - * @param context Context whose model is widened to fit the requested frame size. - * @return The recorder, or nullptr when video is off or unavailable. - */ -std::unique_ptr make_video_recorder(const CliOptions& options, SimulationContext& context) { - if (not options.video_enabled) { - return nullptr; - } - - mjModel* model = context.world.model(); - model->vis.global.offwidth = std::max(model->vis.global.offwidth, options.video.width); - model->vis.global.offheight = std::max(model->vis.global.offheight, options.video.height); - - std::string error; - auto recorder = VideoRecorder::create(model, options.video, error); - - if (recorder == nullptr) { - std::cerr << "video disabled: " << error << '\n'; - return nullptr; - } - - std::cout << "recording " << options.video.path << " at " << options.video.width << "x" << options.video.height - << " " << options.video.fps << " fps, camera '" << options.video.camera << "', one frame every " - << ticks_per_frame(options.video.fps, context.clock) << " ticks\n"; - - return recorder; -} -} // namespace - -Application::Application(const CliOptions& options, FirmwareThread::Program program, PoolTelemetry pool) : - options{options}, - context{prepare(options, SimulationContext::instance())}, - firmware{std::move(program)}, - pool{pool}, - telemetry{this->context.serial}, - scenario{options.scenario, this->context.proxy_state, this->telemetry, this->context.clock, board_delays()}, - recorder{ - this->context.world, this->context.proxy_state, pool == PoolTelemetry::SERVED ? &this->telemetry : nullptr, - options.out / "data.csv" - }, - video{make_video_recorder(options, this->context)}, - viewer{make_viewer(options, this->context, this->telemetry)}, - monitor{make_monitor(options, this->context.serial)}, - simulation{this->context, this->firmware} { - this->context.firmware = &this->firmware; - - if (this->pool == PoolTelemetry::NOT_SERVED and options.scenario.command != Command::NONE) { - throw std::runtime_error("this program does not serve the variable pool, so --command cannot be delivered"); - } - - this->simulation.add_listener(CrashReporter::instance()); - this->simulation.add_listener(this->scenario); - - if (this->monitor != nullptr) { - this->simulation.add_listener(*this->monitor); - } - - if (this->pool == PoolTelemetry::SERVED) { - this->telemetry.request_variable_map(); - this->simulation.add_listener(this->telemetry); - } - - this->simulation.add_listener(this->recorder); - - if (this->video != nullptr) { - this->video->attach( - this->context.world, this->telemetry, ticks_per_frame(options.video.fps, this->context.clock) - ); - this->simulation.add_listener(*this->video); - } - - if (this->viewer != nullptr) { - this->simulation.add_listener(*this->viewer); - } -} - -Application::~Application() { - this->context.firmware = nullptr; -} - -void Application::run(std::span arguments) { - const auto wall_start = std::chrono::steady_clock::now(); - - CrashReporter::install(); - this->simulation.run(this->options.ticks); - - const auto wall_seconds = std::chrono::duration(std::chrono::steady_clock::now() - wall_start).count(); - const int64_t warnings = GroundTruth::warnings_total(this->context.world); - - const RunMetadata metadata{ - .firmware_sha = MICRAS_FIRMWARE_SHA, - .model_path = this->options.model.string(), - .model_sha256 = RunMetadata::sha256_of(this->options.model), - .maze_path = this->options.maze, - .mujoco_version = mj_versionString(), - .compiler = MICRAS_COMPILER_ID " " MICRAS_COMPILER_VERSION, - .build_type = MICRAS_BUILD_TYPE, - .args = RunMetadata::join_args(arguments), - .loop_time_us = loop_time_us, - .timestep = this->context.world.timestep(), - .steps_per_tick = this->context.clock.steps_per_tick(), - .requested_ticks = this->options.ticks, - .ticks = this->simulation.completed_ticks(), - .sim_time = this->context.world.time(), - .final_z = this->recorder.last_z(), - .pool_columns = this->recorder.pool_column_count(), - .telemetry_resyncs = this->telemetry.resync_count(), - .warnings_total = warnings, - .interactive = this->viewer != nullptr and this->viewer->was_interactive(), - }; - metadata.write(this->options.out / "meta.json"); - - std::cout << "ticks=" << this->simulation.completed_ticks() << "/" << this->options.ticks - << " sim_time=" << this->context.world.time() << "s wall=" << wall_seconds - << "s pool_columns=" << this->recorder.pool_column_count() << " warnings=" << warnings; - - if (this->video != nullptr) { - std::cout << " video_frames=" << this->video->frame_count(); - } - - std::cout << '\n'; -} - -int Application::main(std::span arguments, const FirmwareThread::Program& program, PoolTelemetry pool) { - try { - std::cout << "MuJoCo " << mj_versionString() << '\n'; - - if (mj_version() != mjVERSION_HEADER) { - std::cerr << "MuJoCo library/header version mismatch: " << mj_version() << " != " << mjVERSION_HEADER - << '\n'; - return 1; - } - - const CliOptions options = Cli::parse(arguments, loop_time_us); - std::filesystem::create_directories(options.out); - - Application application(options, program, pool); - application.run(arguments); - return 0; - } catch (const std::exception& error) { - std::cerr << "error: " << error.what() << '\n' << Cli::usage(); - return 1; - } -} -} // namespace micras::sim diff --git a/src/sim/app/cli.cpp b/src/sim/app/cli.cpp deleted file mode 100644 index 315bf9b..0000000 --- a/src/sim/app/cli.cpp +++ /dev/null @@ -1,339 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include - -#include "micras/sim/app/cli.hpp" -#include "micras/sim/core/clock.hpp" - -namespace micras::sim { -namespace { -/** - * @brief Read the value that follows an option. - * - * @param arguments Whole command line. - * @param index Index of the option, advanced to the value. - * @return The value. - */ -std::string take_value(std::span arguments, std::size_t& index) { - if (index + 1 >= arguments.size()) { - throw std::runtime_error("missing value for option " + std::string(arguments[index])); - } - - return arguments[++index]; -} - -/** - * @brief Parse the --dip option into the scenario switch states. - * - * @param value Comma separated list of name=0|1 pairs. - * @param script Script to fill, indexed as in Interface::DipSwitchPins. - */ -void parse_dip(const std::string& value, ScenarioScript& script) { - const std::array names{"fan", "diagonal", "boost", "risky"}; - std::size_t start = 0; - - while (start <= value.size()) { - const std::size_t comma = value.find(',', start); - const std::string item = value.substr(start, comma == std::string::npos ? std::string::npos : comma - start); - start = comma == std::string::npos ? value.size() + 1 : comma + 1; - - if (item.empty()) { - continue; - } - - const std::size_t equals = item.find('='); - - if (equals == std::string::npos) { - throw std::runtime_error("malformed --dip entry '" + item + "', expected name=0|1"); - } - - const std::string name = item.substr(0, equals); - const std::string state = item.substr(equals + 1); - const auto* const slot = std::ranges::find(names, name); - - if (slot == names.end()) { - throw std::runtime_error("unknown dip switch '" + name + "'"); - } - - if (state != "0" and state != "1") { - throw std::runtime_error("dip switch '" + name + "' must be 0 or 1"); - } - - script.dip.at(static_cast(slot - names.begin())) = state == "1"; - } -} - -/** - * @brief Parse a number, naming the option and rejecting trailing rubbish. - * - * @tparam T Type to parse. - * @param value Number as spelled on the command line. - * @param option Name of the option, for the error message. - * @return The number. - */ -template -T require_number(const std::string& value, const std::string& option) { - T number{}; - const std::span digits(value); - const auto result = std::from_chars(digits.data(), digits.data() + digits.size(), number); - - if (result.ec != std::errc{} or result.ptr != digits.data() + digits.size()) { - throw std::runtime_error(option + " expects a number, got '" + value + "'"); - } - - return number; -} - -/** - * @brief Parse a size spelled as x. - * - * @param value Size as spelled on the command line. - * @param option Name of the option, for the error message. - * @param width Width to fill. - * @param height Height to fill. - */ -void parse_size(const std::string& value, const std::string& option, int& width, int& height) { - const std::size_t cross = value.find('x'); - - if (cross == std::string::npos) { - throw std::runtime_error(option + " must look like 1280x720, got " + value); - } - - width = require_number(value.substr(0, cross), option); - height = require_number(value.substr(cross + 1), option); -} - -/** - * @brief Options being built, plus what the parser has to remember about them. - */ -struct ParsedOptions { - CliOptions parsed; - bool ticks_given{false}; -}; - -/** - * @brief Reads the value that follows the option being applied. - */ -using ValueReader = std::function; - -/** - * @brief Parse a command name, naming the offender when it is unknown. - * - * @param value Name as spelled on the command line. - * @return The command. - */ -Command require_command(const std::string& value) { - const auto command = parse_command(value); - - if (not command.has_value()) { - throw std::runtime_error("unknown --command " + value); - } - - return *command; -} - -/** - * @brief Parse a button press name, naming the offender when it is unknown. - * - * @param value Name as spelled on the command line. - * @return The press. - */ -ButtonPress require_button_press(const std::string& value) { - const auto press = parse_button_press(value); - - if (not press.has_value()) { - throw std::runtime_error("unknown --button " + value); - } - - return *press; -} - -/** - * @brief Apply one of the window options. - * - * @param argument Name of the option. - * @param value Reader of the value that follows it. - * @param options Options to fill. - * @return False when the name is not a window option. - */ -bool apply_viewer_option(const std::string& argument, const ValueReader& value, CliOptions& options) { - if (argument == "--viewer") { - options.viewer_enabled = true; - } else if (argument == "--viewer-camera") { - options.viewer.camera = value(); - } else if (argument == "--viewer-fps") { - options.viewer_fps = require_number(value(), argument); - } else if (argument == "--viewer-size") { - parse_size(value(), "--viewer-size", options.viewer.width, options.viewer.height); - } else if (argument == "--monitor") { - options.monitor_enabled = true; - } else if (argument == "--monitor-port") { - options.monitor_port = require_number(value(), argument); - } else { - return false; - } - - return true; -} - -/** - * @brief Apply one of the recording options. - * - * @param argument Name of the option. - * @param value Reader of the value that follows it. - * @param options Options to fill. - * @return False when the name is not a recording option. - */ -bool apply_video_option(const std::string& argument, const ValueReader& value, CliOptions& options) { - if (argument == "--video") { - options.video.path = value(); - options.video_enabled = true; - } else if (argument == "--video-fps") { - options.video.fps = require_number(value(), argument); - } else if (argument == "--video-camera") { - options.video.camera = value(); - } else if (argument == "--video-size") { - parse_size(value(), "--video-size", options.video.width, options.video.height); - } else { - return false; - } - - return true; -} - -/** - * @brief Reject an option combination the run could not honour. - * - * @param options Options as parsed. - * @param ticks_given Whether --ticks was given. - */ -void validate(const CliOptions& options, bool ticks_given) { - if (options.model.empty()) { - throw std::runtime_error("--model is required"); - } - - if (options.out.empty()) { - throw std::runtime_error("--out is required"); - } - - if (ticks_given) { - if (options.ticks == 0) { - throw std::runtime_error("--ticks must be at least 1"); - } - } else if (not(options.seconds > 0.0)) { - throw std::runtime_error("--seconds must be strictly positive, got " + std::to_string(options.seconds)); - } - - if (options.viewer_enabled) { - if (options.viewer.width < 1 or options.viewer.height < 1) { - throw std::runtime_error("--viewer-size must be strictly positive"); - } - - if (options.viewer_fps < 1) { - throw std::runtime_error("--viewer-fps must be at least 1"); - } - } - - if (options.monitor_enabled and (options.monitor_port < 1 or options.monitor_port > 65535)) { - throw std::runtime_error("--monitor-port must be a TCP port"); - } - - if (not options.video_enabled) { - return; - } - - if (options.video.fps < 1) { - throw std::runtime_error("--video-fps must be at least 1"); - } - - if (options.video.width < 1 or options.video.height < 1) { - throw std::runtime_error("--video-size must be strictly positive"); - } -} - -/** - * @brief Apply one option to the options being built. - * - * @param argument Name of the option. - * @param value Reader of the value that follows it. - * @param options Options to fill. - * @return False when the name is not an option at all. - */ -bool apply_option(const std::string& argument, const ValueReader& value, ParsedOptions& options) { - if (argument == "--model") { - options.parsed.model = value(); - } else if (argument == "--out") { - options.parsed.out = value(); - } else if (argument == "--maze") { - options.parsed.maze = value(); - } else if (argument == "--seconds") { - options.parsed.seconds = require_number(value(), argument); - } else if (argument == "--ticks") { - options.parsed.ticks = require_number(value(), argument); - options.ticks_given = true; - } else if (argument == "--command") { - options.parsed.scenario.command = require_command(value()); - } else if (argument == "--command-at") { - options.parsed.scenario.command_at = require_number(value(), argument); - } else if (argument == "--dip") { - parse_dip(value(), options.parsed.scenario); - } else if (argument == "--button") { - options.parsed.scenario.button = require_button_press(value()); - } else if (argument == "--button-at") { - options.parsed.scenario.button_at = require_number(value(), argument); - } else if (argument == "--no-fan") { - options.parsed.scenario.fan_enabled = false; - } else { - return apply_video_option(argument, value, options.parsed) or - apply_viewer_option(argument, value, options.parsed); - } - - return true; -} -} // namespace - -std::string Cli::usage() { - return "usage: micras_simulation --model --out [--seconds | --ticks ]\n" - " [--maze ] [--command none|explore|solve|calibrate] [--command-at ]\n" - " [--dip fan=0,diagonal=0,boost=0,risky=0]\n" - " [--button none|short|long|extra_long] [--button-at ] [--no-fan]\n" - " [--video ] [--video-fps ] [--video-camera |free]\n" - " [--video-size x]\n" - " [--viewer] [--viewer-camera |free] [--viewer-fps ]\n" - " [--viewer-size x]\n" - " [--monitor] [--monitor-port ]\n"; -} - -CliOptions Cli::parse(std::span arguments, uint32_t loop_time_us) { - ParsedOptions options; - - for (std::size_t i = 1; i < arguments.size(); i++) { - const std::string argument = arguments[i]; - const ValueReader value = [&arguments, &i] { return take_value(arguments, i); }; - - if (not apply_option(argument, value, options)) { - throw std::runtime_error("unknown option " + argument); - } - } - - validate(options.parsed, options.ticks_given); - - if (not options.ticks_given) { - options.parsed.ticks = Clock::total_ticks(options.parsed.seconds, loop_time_us); - - if (options.parsed.ticks == 0) { - throw std::runtime_error( - "--seconds " + std::to_string(options.parsed.seconds) + " is shorter than one firmware loop period" - ); - } - } - - return options.parsed; -} -} // namespace micras::sim diff --git a/src/sim/app/hardware_test_main.cpp b/src/sim/app/hardware_test_main.cpp deleted file mode 100644 index 91f0f63..0000000 --- a/src/sim/app/hardware_test_main.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @file - * - * @brief Runs one firmware hardware test in the deterministic MuJoCo harness. - */ - -#include - -#include "micras/sim/app/application.hpp" - -/** - * @brief The hardware test's own entry point, renamed at compile time. - * - * @param argc Number of main arguments. - * @param argv Main arguments. - * @return The test's exit code, which the harness ignores. - */ -extern int micras_test_main(int argc, char** argv); - -int main(int argc, char** argv) { - return micras::sim::Application::main( - std::span(argv, static_cast(argc)), [argc, argv] { micras_test_main(argc, argv); }, - micras::sim::PoolTelemetry::NOT_SERVED - ); -} diff --git a/src/sim/bridge/web_socket_server.cpp b/src/sim/bridge/web_socket_server.cpp deleted file mode 100644 index 7ea7d7a..0000000 --- a/src/sim/bridge/web_socket_server.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/** - * @file - */ - -#include - -#include "micras/sim/bridge/web_socket_server.hpp" - -#ifdef MICRAS_BRIDGE - - #include - -namespace micras::sim { -/** - * @brief Everything IXWebSocket needs, kept out of the header. - */ -struct WebSocketServer::Impl { - std::unique_ptr server; - BinaryHandler on_binary; -}; - -WebSocketServer::WebSocketServer() : impl{std::make_unique()} { } - -WebSocketServer::~WebSocketServer() { - this->stop(); -} - -void WebSocketServer::set_on_binary(BinaryHandler handler) { - this->impl->on_binary = std::move(handler); -} - -bool WebSocketServer::start(int port, std::string& error) { - this->impl->server = std::make_unique(port); - this->impl->server->disablePerMessageDeflate(); - - this->impl->server->setOnClientMessageCallback( - [this](const std::shared_ptr&, ix::WebSocket&, const ix::WebSocketMessagePtr& message) { - if (message->type != ix::WebSocketMessageType::Message or not message->binary) { - return; - } - - if (this->impl->on_binary) { - const std::span bytes( - reinterpret_cast(message->str.data()), // NOLINT(*-reinterpret-cast): byte view. - message->str.size() - ); - this->impl->on_binary(bytes); - } - } - ); - - const auto result = this->impl->server->listen(); - - if (not result.first) { - error = result.second; - this->impl->server.reset(); - return false; - } - - this->impl->server->start(); - return true; -} - -void WebSocketServer::stop() { - if (this->impl->server == nullptr) { - return; - } - - this->impl->server->stop(); - this->impl->server.reset(); -} - -void WebSocketServer::broadcast(std::span bytes) { - if (this->impl->server == nullptr or bytes.empty()) { - return; - } - - // NOLINTNEXTLINE(*-reinterpret-cast): IXWebSocket takes a std::string as its byte buffer. - const std::string frame(reinterpret_cast(bytes.data()), bytes.size()); - - for (const auto& client : this->impl->server->getClients()) { - client->sendBinary(frame); - } -} - -std::size_t WebSocketServer::client_count() const { - return this->impl->server == nullptr ? 0 : this->impl->server->getClients().size(); -} -} // namespace micras::sim - -#else // MICRAS_BRIDGE - -namespace micras::sim { -/** - * @brief Nothing to hide when the bridge is compiled out. - */ -struct WebSocketServer::Impl { }; - -WebSocketServer::WebSocketServer() : impl{std::make_unique()} { } - -WebSocketServer::~WebSocketServer() = default; - -void WebSocketServer::set_on_binary(BinaryHandler /*handler*/) { } - -bool WebSocketServer::start(int /*port*/, std::string& error) { - error = "this binary was built with -DMICRAS_BRIDGE=OFF"; - return false; -} - -void WebSocketServer::stop() { } - -void WebSocketServer::broadcast(std::span /*bytes*/) { } - -std::size_t WebSocketServer::client_count() const { - return 0; -} -} // namespace micras::sim - -#endif // MICRAS_BRIDGE diff --git a/src/sim/core/mujoco_world.cpp b/src/sim/core/mujoco_world.cpp deleted file mode 100644 index 5aae94c..0000000 --- a/src/sim/core/mujoco_world.cpp +++ /dev/null @@ -1,154 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include - -#include "micras/sim/core/mujoco_world.hpp" - -namespace micras::sim { -namespace { -/** - * @brief Check an index against the array it is about to reach into. - * - * @param index Index to check. - * @param size Size of the array. - * @param what Name of the array, for the error message. - * @return The index, as an unsigned offset. - */ -std::size_t require_index(int index, std::size_t size, const char* what) { - if (index < 0 or static_cast(index) >= size) { - throw std::out_of_range( - std::string(what) + " index " + std::to_string(index) + " is outside the " + std::to_string(size) + - " entries the model has" - ); - } - - return static_cast(index); -} -} // namespace - -MujocoWorld::~MujocoWorld() { - this->unload(); -} - -void MujocoWorld::load(const std::filesystem::path& model_path) { - std::array error{}; - mjModel* loaded = mj_loadXML(model_path.c_str(), nullptr, error.data(), static_cast(error.size())); - - if (loaded == nullptr) { - throw std::runtime_error("failed to load model '" + model_path.string() + "': " + std::string(error.data())); - } - - mjData* allocated = mj_makeData(loaded); - - if (allocated == nullptr) { - mj_deleteModel(loaded); - throw std::runtime_error("failed to allocate mjData for '" + model_path.string() + "'"); - } - - this->unload(); - this->model_handle = loaded; - this->data_handle = allocated; - this->model_handle->opt.disableflags |= mjDSBL_AUTORESET; -} - -void MujocoWorld::reset() { - this->require_loaded(); - mj_resetData(this->model_handle, this->data_handle); - mj_forward(this->model_handle, this->data_handle); -} - -void MujocoWorld::step(int steps) { - this->require_loaded(); - - for (int i = 0; i < steps; i++) { - mj_step(this->model_handle, this->data_handle); - } -} - -int MujocoWorld::require_id(mjtObj type, const std::string& name) const { - this->require_loaded(); - const int id = mj_name2id(this->model_handle, type, name.c_str()); - - if (id < 0) { - throw std::runtime_error("model object not found: '" + name + "'"); - } - - return id; -} - -int MujocoWorld::sensor_address(const std::string& name) const { - const int id = this->require_id(mjOBJ_SENSOR, name); - const std::span addresses( - this->model_handle->sensor_adr, static_cast(this->model_handle->nsensor) - ); - return addresses[static_cast(id)]; -} - -double MujocoWorld::sensor_value(int address) const { - this->require_loaded(); - const std::span readings( - this->data_handle->sensordata, static_cast(this->model_handle->nsensordata) - ); - return readings[require_index(address, readings.size(), "sensordata")]; -} - -void MujocoWorld::set_control(int actuator_id, double value) { - this->require_loaded(); - const std::span controls(this->data_handle->ctrl, static_cast(this->model_handle->nu)); - controls[require_index(actuator_id, controls.size(), "actuator")] = value; -} - -double MujocoWorld::timestep() const { - this->require_loaded(); - return this->model_handle->opt.timestep; -} - -double MujocoWorld::time() const { - this->require_loaded(); - return this->data_handle->time; -} - -const mjModel* MujocoWorld::model() const { - this->require_loaded(); - return this->model_handle; -} - -mjModel* MujocoWorld::model() { - this->require_loaded(); - return this->model_handle; -} - -const mjData* MujocoWorld::data() const { - this->require_loaded(); - return this->data_handle; -} - -mjData* MujocoWorld::data() { - this->require_loaded(); - return this->data_handle; -} - -void MujocoWorld::require_loaded() const { - if (this->model_handle == nullptr or this->data_handle == nullptr) { - throw std::logic_error("the MuJoCo world was used before a model was loaded"); - } -} - -void MujocoWorld::unload() { - if (this->data_handle != nullptr) { - mj_deleteData(this->data_handle); - this->data_handle = nullptr; - } - - if (this->model_handle != nullptr) { - mj_deleteModel(this->model_handle); - this->model_handle = nullptr; - } -} -} // namespace micras::sim diff --git a/src/sim/core/scenario.cpp b/src/sim/core/scenario.cpp deleted file mode 100644 index 68a5b73..0000000 --- a/src/sim/core/scenario.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file - */ - -#include -#include -#include - -#include "micras/sim/core/scenario.hpp" - -namespace micras::sim { -namespace { -/** - * @brief Name each command answers to on the command line. - */ -constexpr std::array, 4> command_names{ - {{"none", Command::NONE}, - {"explore", Command::EXPLORE}, - {"solve", Command::SOLVE}, - {"calibrate", Command::CALIBRATE}} -}; - -/** - * @brief Name each button press answers to on the command line. - */ -constexpr std::array, 4> press_names{ - {{"none", ButtonPress::NONE}, - {"short", ButtonPress::SHORT}, - {"long", ButtonPress::LONG}, - {"extra_long", ButtonPress::EXTRA_LONG}} -}; -} // namespace - -std::optional parse_command(std::string_view name) { - const auto* const entry = std::ranges::find(command_names, name, &std::pair::first); - return entry == command_names.end() ? std::nullopt : std::optional{entry->second}; -} - -std::optional parse_button_press(std::string_view name) { - const auto* const entry = std::ranges::find(press_names, name, &std::pair::first); - return entry == press_names.end() ? std::nullopt : std::optional{entry->second}; -} - -std::string_view command_variable(Command command) { - switch (command) { - case Command::EXPLORE: - return "Explore"; - - case Command::SOLVE: - return "Solve"; - - case Command::CALIBRATE: - return "Calibrate"; - - case Command::NONE: - break; - } - - return {}; -} - -uint32_t Scenario::hold_time_ms(ButtonPress press, const ButtonDelays& delays) { - switch (press) { - case ButtonPress::SHORT: - return delays.long_press / 2; - - case ButtonPress::LONG: - return delays.long_press + 1; - - case ButtonPress::EXTRA_LONG: - return delays.extra_long_press + 1; - - case ButtonPress::NONE: - break; - } - - return 0; -} - -Scenario::Scenario( - const ScenarioScript& script, ProxyState& state, Telemetry& telemetry, const Clock& clock, - const ButtonDelays& delays -) : - script{script}, - state{state}, - telemetry{telemetry}, - command_tick{std::max(1, clock.tick_at(script.command_at))}, - button_press_tick{clock.tick_at(script.button_at)}, - button_release_tick{this->button_press_tick + clock.ticks_for_elapsed_ms(hold_time_ms(script.button, delays))}, - command_sent{script.command == Command::NONE} { } - -void Scenario::on_start(const Simulation& /*simulation*/) { - this->state.overrides.fan_enabled = this->script.fan_enabled; - this->state.interface_input.dip_switches = this->script.dip; -} - -RunControl Scenario::on_before_tick(const Simulation& simulation) { - const uint64_t tick = simulation.tick(); - - if (this->state.interface_input.driven_by_human) { - return RunControl::RUN; - } - - if (this->script.button != ButtonPress::NONE) { - this->state.interface_input.button_pressed = - tick >= this->button_press_tick and tick < this->button_release_tick; - } - - if (this->command_sent or tick < this->command_tick or not this->telemetry.has_variable_map()) { - return RunControl::RUN; - } - - const std::string name{command_variable(this->script.command)}; - const std::optional id = this->telemetry.find_variable_id(name); - - if (not id.has_value()) { - throw std::runtime_error("firmware pool has no variable named " + name); - } - - this->telemetry.send_variable(*id, {0x01}); - this->command_sent = true; - std::cout << "injected " << name << "=true at tick " << tick << " (id " << *id << ")\n"; - - return RunControl::RUN; -} -} // namespace micras::sim diff --git a/src/sim/core/serial_bus.cpp b/src/sim/core/serial_bus.cpp deleted file mode 100644 index f483318..0000000 --- a/src/sim/core/serial_bus.cpp +++ /dev/null @@ -1,51 +0,0 @@ -/** - * @file - */ - -#include - -#include "micras/sim/core/serial_bus.hpp" - -namespace micras::sim { -void SerialBus::add_listener(ISerialListener& listener) { - if (std::ranges::find(this->listeners, &listener) == this->listeners.end()) { - this->listeners.push_back(&listener); - } -} - -void SerialBus::remove_listener(const ISerialListener& listener) { - std::erase(this->listeners, &listener); -} - -void SerialBus::send_from_firmware(std::span bytes) { - const std::vector delivering = this->listeners; - - for (ISerialListener* listener : delivering) { - listener->on_firmware_bytes(bytes); - } -} - -void SerialBus::queue_for_firmware(std::span packet) { - if (this->to_firmware.size() >= max_pending_packets) { - this->dropped++; - return; - } - - this->to_firmware.emplace_back(packet.begin(), packet.end()); -} - -std::vector SerialBus::take_for_firmware() { - if (this->to_firmware.empty()) { - return {}; - } - - std::vector packet = std::move(this->to_firmware.front()); - this->to_firmware.pop_front(); - return packet; -} - -void SerialBus::clear() { - this->to_firmware.clear(); - this->dropped = 0; -} -} // namespace micras::sim diff --git a/src/sim/core/simulation_context.cpp b/src/sim/core/simulation_context.cpp deleted file mode 100644 index 7753f45..0000000 --- a/src/sim/core/simulation_context.cpp +++ /dev/null @@ -1,31 +0,0 @@ -/** - * @file - */ - -#include - -#include "micras/sim/core/firmware_thread.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::sim { -SimulationContext& SimulationContext::instance() { - static SimulationContext context; - return context; -} - -SimulationContext& require_context(SimulationContext* context) { - if (context == nullptr) { - throw std::logic_error("a proxy was configured without a simulation context"); - } - - return *context; -} - -void yield_tick() { - FirmwareThread* firmware = SimulationContext::instance().firmware; - - if (firmware != nullptr) { - firmware->yield_tick(); - } -} -} // namespace micras::sim diff --git a/src/sim/recording/csv_recorder.cpp b/src/sim/recording/csv_recorder.cpp deleted file mode 100644 index 4011380..0000000 --- a/src/sim/recording/csv_recorder.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/** - * @file - */ - -#include -#include - -#include "micras/sim/recording/csv_recorder.hpp" - -namespace micras::sim { -CsvRecorder::CsvRecorder( - const MujocoWorld& world, const ProxyState& proxy_state, const Telemetry* telemetry, - const std::filesystem::path& path -) : - telemetry{telemetry}, proxy_state{proxy_state}, ground_truth{world}, writer{path} { } - -std::vector CsvRecorder::proxy_columns() { - std::vector columns{"proxy_left_command", "proxy_right_command", "proxy_fan_speed", - "proxy_encoder_left", "proxy_encoder_right", "proxy_gyro_x", - "proxy_gyro_y", "proxy_gyro_z", "proxy_accel_x", - "proxy_accel_y", "proxy_accel_z", "proxy_wall_adc_0", - "proxy_wall_adc_1", "proxy_wall_adc_2", "proxy_wall_adc_3", - "proxy_button_pressed"}; - - for (std::size_t i = 0; i < InterfaceInput::dip_switch_count; i++) { - columns.push_back("proxy_dip_" + std::to_string(i)); - } - - return columns; -} - -std::string CsvRecorder::column_name(const std::string& name) { - std::string column; - column.reserve(name.size()); - - for (const char character : name) { - if (std::isalnum(static_cast(character)) != 0) { - column.push_back(static_cast(std::tolower(static_cast(character)))); - } else if (not column.empty() and column.back() != '_') { - column.push_back('_'); - } - } - - while (not column.empty() and column.back() == '_') { - column.pop_back(); - } - - return column; -} - -void CsvRecorder::write_header() { - if (this->telemetry != nullptr and not this->telemetry->has_variable_map()) { - const std::string reason = this->telemetry->map_failure_count() > 0 ? - "the firmware answered the variable map request with " + - std::to_string(this->telemetry->map_failure_count()) + - " malformed response(s)" : - "the firmware did not answer the variable map request on tick 0"; - throw std::runtime_error(reason + "; the CSV header would silently lose every pool column"); - } - - for (const auto& variable : this->pool_variables()) { - const std::string base = column_name(variable.name); - const std::vector suffixes = VariableMap::type_column_suffixes(variable.type); - - if (suffixes.empty()) { - this->pool_columns.push_back({variable.id, 0, base}); - continue; - } - - for (std::size_t component = 0; component < suffixes.size(); component++) { - this->pool_columns.push_back({variable.id, component, base + '_' + suffixes[component]}); - } - } - - std::vector columns = GroundTruth::columns(); - - for (const auto& column : this->pool_columns) { - columns.push_back(column.name); - } - - for (const auto& column : proxy_columns()) { - columns.push_back(column); - } - - this->writer.write_header(columns); - this->header_written = true; -} - -void CsvRecorder::on_after_tick(const Simulation& simulation) { - this->sample(simulation.tick()); -} - -std::vector CsvRecorder::pool_variables() const { - return this->telemetry == nullptr ? std::vector{} : - this->telemetry->variable_map().monitoring_variables(); -} - -void CsvRecorder::sample(uint64_t tick) { - if (not this->header_written) { - this->write_header(); - } - - std::vector cells = this->ground_truth.sample(tick); - - if (cells.size() != GroundTruth::columns().size()) { - throw std::logic_error("the ground truth sampled a different number of cells than it has columns"); - } - - for (const auto& column : this->pool_columns) { - cells.emplace_back(this->telemetry->value(column.id, column.component)); - } - - const Actuators& actuators = this->proxy_state.actuators; - const Sensors& sensors = this->proxy_state.sensors; - - cells.emplace_back(actuators.left_command); - cells.emplace_back(actuators.right_command); - cells.emplace_back(actuators.fan_speed); - - for (const float position : sensors.encoder_positions) { - cells.emplace_back(position); - } - - for (const float rate : sensors.angular_velocity) { - cells.emplace_back(rate); - } - - for (const float acceleration : sensors.linear_acceleration) { - cells.emplace_back(acceleration); - } - - for (const float reading : sensors.wall_adc_readings) { - cells.emplace_back(reading); - } - - const InterfaceInput& input = this->proxy_state.interface_input; - - cells.emplace_back(static_cast(input.button_pressed)); - - for (const bool state : input.dip_switches) { - cells.emplace_back(static_cast(state)); - } - - this->writer.write_row(cells); -} -} // namespace micras::sim diff --git a/src/sim/recording/ground_truth.cpp b/src/sim/recording/ground_truth.cpp deleted file mode 100644 index 3df039d..0000000 --- a/src/sim/recording/ground_truth.cpp +++ /dev/null @@ -1,226 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include -#include - -#include "micras/sim/recording/ground_truth.hpp" - -// NOLINTBEGIN(cppcoreguidelines-pro-bounds-pointer-arithmetic): mjData exposes flat C arrays indexed by id. - -namespace micras::sim { -namespace { -/** - * @brief Nominal firmware loop period in seconds, written on every row. - */ -constexpr double firmware_loop_time = 1042.0e-6; - -/** - * @brief Convert a MuJoCo quaternion to roll, pitch and yaw. - * - * @param quaternion Quaternion in w, x, y, z order. - * @return Array with roll, pitch and yaw in radians. - */ -std::array to_euler(const mjtNum* quaternion) { - const double scalar = quaternion[0]; - const double x = quaternion[1]; - const double y = quaternion[2]; - const double z = quaternion[3]; - - const double roll = std::atan2(2.0 * (scalar * x + y * z), 1.0 - 2.0 * (x * x + y * y)); - const double sin_pitch = std::clamp(2.0 * (scalar * y - z * x), -1.0, 1.0); - const double pitch = std::asin(sin_pitch); - const double yaw = std::atan2(2.0 * (scalar * z + x * y), 1.0 - 2.0 * (y * y + z * z)); - - return {roll, pitch, yaw}; -} - -/** - * @brief Resolve the free joint the chassis floats on. - * - * @param model Loaded model. - * @param body_id Id of the robot body. - * @return Address of the first velocity dof of the free joint. - */ -int free_joint_dof(const mjModel* model, int body_id) { - if (model->body_jntnum[body_id] < 1) { - throw std::runtime_error("body 'micras' has no joint, a free joint is required"); - } - - const int root_joint = model->body_jntadr[body_id]; - - if (model->jnt_type[root_joint] != mjJNT_FREE) { - throw std::runtime_error( - "the first joint of body 'micras' must be a free joint, got mjtJoint " + - std::to_string(model->jnt_type[root_joint]) - ); - } - - return model->jnt_dofadr[root_joint]; -} -} // namespace - -GroundTruth::GroundTruth(const MujocoWorld& world) : - world{world}, - body_id{world.require_id(mjOBJ_BODY, "micras")}, - left_wheel_geom{world.require_id(mjOBJ_GEOM, "left_wheel")}, - right_wheel_geom{world.require_id(mjOBJ_GEOM, "right_wheel")}, - caster_geom{world.require_id(mjOBJ_GEOM, "front wheel")}, - base_geom{world.require_id(mjOBJ_GEOM, "base")}, - left_actuator{world.require_id(mjOBJ_ACTUATOR, "motor_left")}, - right_actuator{world.require_id(mjOBJ_ACTUATOR, "motor_right")}, - free_joint_qvel{free_joint_dof(world.model(), this->body_id)} { - const mjModel* model = world.model(); - const int left_wheel = world.require_id(mjOBJ_JOINT, "left_wheel"); - const int right_wheel = world.require_id(mjOBJ_JOINT, "right_wheel"); - - this->left_wheel_qvel = model->jnt_dofadr[left_wheel]; - this->right_wheel_qvel = model->jnt_dofadr[right_wheel]; - this->left_wheel_qpos = model->jnt_qposadr[left_wheel]; - this->right_wheel_qpos = model->jnt_qposadr[right_wheel]; -} - -int64_t GroundTruth::warnings_total(const MujocoWorld& world) { - int64_t warnings = 0; - - for (const mjWarningStat& warning : std::span(world.data()->warning)) { - warnings += warning.number; - } - - return warnings; -} - -std::vector GroundTruth::columns() { - return { - "tick", - "sim_time", - "fw_loop_time", - "x", - "y", - "z", - "roll", - "pitch", - "yaw", - "vx_world", - "vy_world", - "vz_world", - "wz_body", - "v_forward", - "wheel_qvel_left", - "wheel_qvel_right", - "ctrl_left", - "ctrl_right", - "act_force_left", - "act_force_right", - "left_ncon", - "left_fn", - "left_slip", - "left_penetration", - "right_ncon", - "right_fn", - "right_slip", - "right_penetration", - "caster_ncon", - "ncon_total", - "solver_niter", - "warnings_total", - "wheel_qpos_left", - "wheel_qpos_right", - "caster_fn", - "base_ncon" - }; -} - -GroundTruth::ContactStats GroundTruth::collect_contacts(int geom_id) const { - const mjModel* model = this->world.model(); - const mjData* data = this->world.data(); - ContactStats stats; - - for (int i = 0; i < data->ncon; i++) { - const mjContact& contact = data->contact[i]; - - if (contact.efc_address < 0) { - continue; - } - - if (contact.geom[0] != geom_id and contact.geom[1] != geom_id) { - continue; - } - - stats.count++; - stats.penetration = stats.count == 1 ? contact.dist : std::min(stats.penetration, contact.dist); - - std::array force{}; - mj_contactForce(model, data, i, force.data()); - stats.normal_force += force[0]; - - if (contact.dim >= 3) { - const int address = contact.efc_address; - stats.slip = std::max(stats.slip, std::hypot(data->efc_vel[address + 1], data->efc_vel[address + 2])); - } - } - - return stats; -} - -std::vector GroundTruth::sample(uint64_t tick) { - const mjData* data = this->world.data(); - const mjtNum* position = data->xpos + 3L * this->body_id; - const auto euler = to_euler(data->xquat + 4L * this->body_id); - const mjtNum* velocity = data->qvel + this->free_joint_qvel; - - this->last_body_z = position[2]; - - const double forward_speed = -velocity[0] * std::sin(euler[2]) + velocity[1] * std::cos(euler[2]); - - const ContactStats left = this->collect_contacts(this->left_wheel_geom); - const ContactStats right = this->collect_contacts(this->right_wheel_geom); - const ContactStats caster = this->collect_contacts(this->caster_geom); - const ContactStats base = this->collect_contacts(this->base_geom); - - return { - tick, - data->time, - firmware_loop_time, - position[0], - position[1], - position[2], - euler[0], - euler[1], - euler[2], - velocity[0], - velocity[1], - velocity[2], - velocity[5], - forward_speed, - data->qvel[this->left_wheel_qvel], - data->qvel[this->right_wheel_qvel], - data->ctrl[this->left_actuator], - data->ctrl[this->right_actuator], - data->actuator_force[this->left_actuator], - data->actuator_force[this->right_actuator], - static_cast(left.count), - left.normal_force, - left.slip, - left.penetration, - static_cast(right.count), - right.normal_force, - right.slip, - right.penetration, - static_cast(caster.count), - static_cast(data->ncon), - static_cast(data->solver_niter[0]), - warnings_total(this->world), - data->qpos[this->left_wheel_qpos], - data->qpos[this->right_wheel_qpos], - caster.normal_force, - static_cast(base.count), - }; -} -} // namespace micras::sim - -// NOLINTEND(cppcoreguidelines-pro-bounds-pointer-arithmetic) diff --git a/src/sim/telemetry/packet_framer.cpp b/src/sim/telemetry/packet_framer.cpp deleted file mode 100644 index 5ea15f8..0000000 --- a/src/sim/telemetry/packet_framer.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/** - * @file - */ - -#include "micras/comm/packet.hpp" -#include "micras/sim/telemetry/packet_framer.hpp" - -namespace micras::sim { -namespace { -/** - * @brief Offset of the first payload byte from the header byte. - */ -constexpr std::size_t payload_offset{6}; -} // namespace - -void PacketFramer::push(std::span bytes) { - this->buffer.insert(this->buffer.end(), bytes.begin(), bytes.end()); -} - -std::vector> PacketFramer::drain() { - std::vector> packets; - std::size_t consumed = 0; - std::size_t index = 0; - - while (index < this->buffer.size()) { - if (this->buffer[index] != comm::Packet::header_byte) { - index++; - continue; - } - - if (index + payload_offset > this->buffer.size()) { - break; - } - - const std::size_t tail = this->find_tail(index); - - if (tail >= this->buffer.size()) { - break; - } - - const auto first = this->buffer.begin() + static_cast(index); - const auto last = this->buffer.begin() + static_cast(tail) + 1; - packets.emplace_back(first, last); - index = tail + 1; - consumed = index; - } - - this->buffer.erase(this->buffer.begin(), this->buffer.begin() + static_cast(consumed)); - this->resync_if_overflowed(); - - return packets; -} - -std::size_t PacketFramer::find_tail(std::size_t start) const { - std::size_t index = start + payload_offset; - - while (index < this->buffer.size()) { - if (this->buffer[index] == comm::Packet::escape_byte) { - index += 2; - continue; - } - - if (this->buffer[index] == comm::Packet::tail_byte) { - return index; - } - - index++; - } - - return this->buffer.size(); -} - -void PacketFramer::resync_if_overflowed() { - if (this->buffer.size() <= max_buffer_size) { - return; - } - - this->buffer.clear(); - this->resyncs++; -} -} // namespace micras::sim diff --git a/src/sim/telemetry/telemetry.cpp b/src/sim/telemetry/telemetry.cpp deleted file mode 100644 index fc21157..0000000 --- a/src/sim/telemetry/telemetry.cpp +++ /dev/null @@ -1,286 +0,0 @@ -/** - * @file - */ - -#include -#include -#include -#include - -#include "micras/comm/packet.hpp" -#include "micras/sim/telemetry/telemetry.hpp" - -namespace micras::sim { -namespace { -constexpr double not_a_number = std::numeric_limits::quiet_NaN(); - -/** - * @brief Read a little-endian unsigned 16 bit integer. - * - * @param data Source buffer. - * @param offset Offset of the first byte. - * @return Decoded value. - */ -uint16_t read_u16_le(const std::vector& data, std::size_t offset) { - return static_cast(data.at(offset) | (data.at(offset + 1) << 8)); -} - -/** - * @brief Read a little-endian 32 bit float. - * - * @param payload Source buffer. - * @param offset Offset of the first byte. - * @return Decoded value. - */ -double read_f32_le(const std::vector& payload, std::size_t offset) { - float value{}; - std::memcpy(&value, &payload.at(offset), sizeof(value)); - return value; -} - -/** - * @brief Strip the namespace qualification off a firmware type name. - * - * @param type Type name as reported in the variable map response. - * @return Last component of the qualified name. - */ -std::string_view bare_type(const std::string& type) { - const std::size_t separator = type.rfind("::"); - return separator == std::string::npos ? std::string_view{type} : std::string_view{type}.substr(separator + 2); -} - -/** - * @brief Read one length-prefixed string out of the map payload. - * - * @param payload Map response payload. - * @param offset Offset of the length byte, advanced past the string. - * @return The string, or nullopt if the payload ends early. - */ -std::optional read_string(const std::vector& payload, std::size_t& offset) { - if (offset >= payload.size()) { - return std::nullopt; - } - - const std::size_t length = payload.at(offset++); - - if (offset + length > payload.size()) { - return std::nullopt; - } - - const auto first = payload.begin() + static_cast(offset); - std::string text(first, first + static_cast(length)); - offset += length; - return text; -} - -/** - * @brief Read one pool variable entry out of the map payload. - * - * @param payload Map response payload. - * @param offset Offset of the entry, advanced past it. - * @return The variable, or nullopt if the payload ends early. - */ -std::optional read_variable(const std::vector& payload, std::size_t& offset) { - if (offset + 3 > payload.size()) { - return std::nullopt; - } - - PoolVariable variable{}; - variable.id = read_u16_le(payload, offset); - offset += 2; - - const std::optional name = read_string(payload, offset); - - if (not name.has_value()) { - return std::nullopt; - } - - const std::optional type = read_string(payload, offset); - - if (not type.has_value() or offset >= payload.size()) { - return std::nullopt; - } - - variable.name = name.value(); - variable.type = type.value(); - variable.read_only = payload.at(offset++) != 0; - return variable; -} -} // namespace - -std::optional VariableMap::parse(const std::vector& payload) { - if (payload.size() < 2) { - return std::nullopt; - } - - VariableMap map; - - std::vector variables; - const uint16_t count = read_u16_le(payload, 0); - std::size_t offset = 2; - - for (uint16_t i = 0; i < count; i++) { - const std::optional variable = read_variable(payload, offset); - - if (not variable.has_value()) { - return std::nullopt; - } - - variables.push_back(*variable); - } - - std::sort(variables.begin(), variables.end(), [](const PoolVariable& left, const PoolVariable& right) { - return left.id < right.id; - }); - - for (const auto& variable : variables) { - map.types[variable.id] = variable.type; - map.ids_by_name[variable.name] = variable.id; - - if (variable.read_only) { - map.monitoring.push_back(variable); - } - } - - return map; -} - -std::vector VariableMap::type_column_suffixes(const std::string& type) { - const std::string_view bare = bare_type(type); - - if (bare == "GridPose") { - return {"x", "y", "side"}; - } - - if (bare == "State") { - return {"x", "y", "orientation", "v_linear", "v_angular"}; - } - - return {}; -} - -std::optional VariableMap::find_id(const std::string& name) const { - const auto entry = this->ids_by_name.find(name); - return entry == this->ids_by_name.end() ? std::nullopt : std::optional{entry->second}; -} - -std::string VariableMap::type_of(uint16_t id) const { - const auto entry = this->types.find(id); - return entry == this->types.end() ? std::string{} : entry->second; -} - -std::vector PoolValues::decode(const std::string& type, const std::vector& payload) { - const std::string_view bare = bare_type(type); - - if (bare == "float" and payload.size() == sizeof(float)) { - return {read_f32_le(payload, 0)}; - } - - if (bare == "double" and payload.size() == sizeof(double)) { - double value{}; - std::memcpy(&value, payload.data(), sizeof(value)); - return {value}; - } - - if (bare == "bool" and payload.size() == sizeof(bool)) { - return {payload.front() != 0 ? 1.0 : 0.0}; - } - - if (bare == "signed char" and payload.size() == 1) { - return {static_cast(static_cast(payload.front()))}; - } - - if ((bare == "unsigned char" or bare == "char") and payload.size() == 1) { - return {static_cast(payload.front())}; - } - - if (bare == "GridPose" and payload.size() == 3) { - return {static_cast(payload[0]), static_cast(payload[1]), static_cast(payload[2])}; - } - - if (bare == "State" and payload.size() == 5 * sizeof(float)) { - return { - read_f32_le(payload, 0), read_f32_le(payload, 4), read_f32_le(payload, 8), read_f32_le(payload, 12), - read_f32_le(payload, 16) - }; - } - - const std::size_t columns = std::max(1, VariableMap::type_column_suffixes(type).size()); - return std::vector(columns, not_a_number); -} - -void PoolValues::store(uint16_t id, const std::string& type, const std::vector& payload) { - this->values[id] = decode(type, payload); -} - -double PoolValues::get(uint16_t id, std::size_t component) const { - const auto entry = this->values.find(id); - - if (entry == this->values.end() or component >= entry->second.size()) { - return not_a_number; - } - - return entry->second[component]; -} - -Telemetry::Telemetry(SerialBus& serial) : serial{serial} { - this->serial.add_listener(*this); -} - -Telemetry::~Telemetry() { - this->serial.remove_listener(*this); -} - -void Telemetry::on_firmware_bytes(std::span bytes) { - this->framer.push(bytes); -} - -void Telemetry::request_variable_map() { - this->serial.queue_for_firmware(comm::Packet(comm::Packet::MessageType::SERIAL_VARIABLE_MAP_REQUEST).serialize()); -} - -void Telemetry::send_variable(uint16_t id, const std::vector& payload) { - this->serial.queue_for_firmware(comm::Packet(comm::Packet::MessageType::SERIAL_VARIABLE, id, payload).serialize()); -} - -void Telemetry::on_after_tick(const Simulation& /*simulation*/) { - this->poll(); -} - -void Telemetry::poll() { - for (const auto& raw : this->framer.drain()) { - this->consume(raw); - } -} - -double Telemetry::value_of(const std::string& name) const { - const std::optional id = this->map.find_id(name); - return id.has_value() ? this->values.get(*id) : not_a_number; -} - -void Telemetry::consume(const std::vector& raw) { - const comm::Packet packet(raw); - - switch (packet.get_type()) { - case comm::Packet::MessageType::SERIAL_VARIABLE_MAP_RESPONSE: { - const std::optional parsed = VariableMap::parse(packet.get_payload()); - - if (parsed.has_value()) { - this->map = *parsed; - this->map_received = true; - } else { - this->map_failures++; - } - - break; - } - - case comm::Packet::MessageType::SERIAL_VARIABLE: - this->values.store(packet.get_id(), this->map.type_of(packet.get_id()), packet.get_payload()); - break; - - default: - break; - } -} -} // namespace micras::sim diff --git a/src/sim/view/control_panel.cpp b/src/sim/view/control_panel.cpp deleted file mode 100644 index b551bb4..0000000 --- a/src/sim/view/control_panel.cpp +++ /dev/null @@ -1,280 +0,0 @@ -/** - * @file - */ - -#include "micras/sim/view/control_panel.hpp" - -#ifdef MICRAS_VIEWER - - #include - #include - #include - - #include - #include - #include - #include - #include - -namespace micras::sim { -namespace { -/** - * @brief Range of the speed limiter, in simulated seconds per wall second. - * - * @note Zero means no limit, which is what a headless run does. - */ -///@{ -constexpr double speed_minimum{0.0}; -constexpr double speed_maximum{10.0}; -///@} - -/** - * @brief Pool variables the panel plots, in the order they are drawn. - */ -constexpr std::array plotted_variables{ - "Desired Linear Speed", "Odometry Linear Velocity", "Desired Angular Speed", "Odometry Angular Velocity" -}; - -/** - * @brief Names of the DIP switches, as Interface::DipSwitchPins orders them. - */ -constexpr std::array dip_names{"fan", "diagonal", "boost", "risky"}; - -/** - * @brief Name of a Micras::State value. - * - * @note The negated comparison is the NaN guard: an unmapped pool variable - * reads as NaN, and every comparison with NaN is false. - * - * @param state Numeric state as reported by the firmware pool. - * @return State name, or "?" when out of range or not reported yet. - */ -const char* state_name(double state) { - static constexpr std::array names{"INIT", "IDLE", "WAIT_FOR_RUN", "RUN", "WAIT_FOR_CALIBRATE", - "CALIBRATE", "ERROR"}; - const auto index = static_cast(state); - - if (not(state >= 0.0) or index >= names.size()) { - return "?"; - } - - return names.at(index); -} - -/** - * @brief Draw a colour swatch for one addressable LED. - * - * @param label Label shown beside it. - * @param colour Colour to show. - */ -void draw_swatch(const std::string& label, const Rgb& colour) { - const ImVec4 value(colour.red / 255.0F, colour.green / 255.0F, colour.blue / 255.0F, 1.0F); - ImGui::ColorButton(label.c_str(), value, ImGuiColorEditFlags_NoTooltip, ImVec2(24, 24)); - ImGui::SameLine(); - ImGui::TextUnformatted(label.c_str()); -} -} // namespace - -PlotTrace::PlotTrace(std::string variable) : variable{std::move(variable)} { - this->sample_times.reserve(capacity); - this->sample_values.reserve(capacity); -} - -void PlotTrace::sample(double time, const Telemetry& telemetry) { - if (this->sample_times.size() == capacity) { - this->sample_times.erase(this->sample_times.begin()); - this->sample_values.erase(this->sample_values.begin()); - } - - this->sample_times.push_back(time); - this->sample_values.push_back(telemetry.value_of(this->variable)); -} - -ControlPanel::ControlPanel(GLFWwindow* window, ProxyState& state, const Telemetry& telemetry) : - state{state}, telemetry{telemetry} { - IMGUI_CHECKVERSION(); - ImGui::CreateContext(); - ImGui::GetIO().IniFilename = nullptr; - ImPlot::CreateContext(); - ImGui::StyleColorsDark(); - ImGui_ImplGlfw_InitForOpenGL(window, true); - ImGui_ImplOpenGL3_Init("#version 130"); - - for (const char* variable : plotted_variables) { - this->traces.emplace_back(variable); - } -} - -ControlPanel::~ControlPanel() { - ImGui_ImplOpenGL3_Shutdown(); - ImGui_ImplGlfw_Shutdown(); - ImPlot::DestroyContext(); - ImGui::DestroyContext(); -} - -void ControlPanel::take_over() { - this->touched = true; - this->state.interface_input.driven_by_human = true; -} - -void ControlPanel::sample(double time) { - for (PlotTrace& trace : this->traces) { - trace.sample(time, this->telemetry); - } -} - -PanelRequest ControlPanel::draw(const Simulation& simulation, bool paused) { - ImGui_ImplOpenGL3_NewFrame(); - ImGui_ImplGlfw_NewFrame(); - ImGui::NewFrame(); - - ImGui::SetNextWindowPos(ImVec2(10, 10), ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(360, 620), ImGuiCond_FirstUseEver); - ImGui::Begin("micras"); - - ImGui::TextUnformatted(std::format("tick {}", simulation.tick()).c_str()); - ImGui::TextUnformatted(std::format("state {}", state_name(this->telemetry.value_of("FSM State"))).c_str()); - ImGui::Separator(); - - this->request.paused_changed = false; - this->draw_board(paused); - ImGui::Separator(); - this->draw_plots(); - - ImGui::End(); - ImGui::Render(); - ImGui_ImplOpenGL3_RenderDrawData(ImGui::GetDrawData()); - - return this->request; -} - -void ControlPanel::draw_board(bool paused) { - InterfaceInput& input = this->state.interface_input; - - ImGui::TextUnformatted("run"); - bool wanted = paused; - - if (ImGui::Checkbox("paused", &wanted)) { - this->request.paused = wanted; - this->request.paused_changed = true; - } - - ImGui::SameLine(); - - if (ImGui::Button("step")) { - this->request.steps++; - } - - ImGui::SameLine(); - - if (ImGui::Button("quit")) { - this->request.quit = true; - } - - ImGui::SliderScalar("speed", ImGuiDataType_Double, &this->request.speed, &speed_minimum, &speed_maximum, "%.1f x"); - - ImGui::Spacing(); - ImGui::TextUnformatted("board"); - - ImGui::Button("button"); - - if (ImGui::IsItemActivated()) { - this->take_over(); - input.button_pressed = true; - } else if (ImGui::IsItemDeactivated()) { - input.button_pressed = false; - } - - for (std::size_t i = 0; i < dip_names.size(); i++) { - bool switched = input.dip_switches.at(i); - - if (ImGui::Checkbox(dip_names.at(i), &switched)) { - this->take_over(); - input.dip_switches.at(i) = switched; - } - - if (i + 1 < dip_names.size()) { - ImGui::SameLine(); - } - } - - if (ImGui::Checkbox("fan allowed", &this->state.overrides.fan_enabled)) { - this->take_over(); - } - - ImGui::Spacing(); - ImGui::TextUnformatted("shown"); - - const InterfaceOutput& output = this->state.interface_output; - draw_swatch("led", output.led_on ? Rgb{255, 255, 255} : Rgb{40, 40, 40}); - - for (std::size_t i = 0; i < output.argb.size(); i++) { - draw_swatch(std::format("argb {}", i), output.argb.at(i)); - } - - ImGui::TextUnformatted(std::format("buzzer {} Hz", output.buzzer_frequency).c_str()); - - ImGui::Spacing(); - ImGui::TextUnformatted("driven"); - - const Actuators& actuators = this->state.actuators; - ImGui::TextUnformatted( - std::format("wheels {:6.1f} {:6.1f}", actuators.left_command, actuators.right_command).c_str() - ); - ImGui::TextUnformatted(std::format("fan {:6.1f}", actuators.fan_speed).c_str()); - - const Sensors& sensors = this->state.sensors; - ImGui::TextUnformatted(std::format( - "walls {:5.3f} {:5.3f} {:5.3f} {:5.3f}", sensors.wall_adc_readings.at(0), - sensors.wall_adc_readings.at(1), sensors.wall_adc_readings.at(2), - sensors.wall_adc_readings.at(3) - ) - .c_str()); -} - -void ControlPanel::draw_plots() { - for (const PlotTrace& trace : this->traces) { - if (not ImPlot::BeginPlot(trace.name().c_str(), ImVec2(-1, 110))) { - continue; - } - - ImPlot::SetupAxes("s", nullptr, ImPlotAxisFlags_AutoFit, ImPlotAxisFlags_AutoFit); - - if (not trace.times().empty()) { - ImPlot::PlotLine( - trace.name().c_str(), trace.times().data(), trace.values().data(), - static_cast(trace.times().size()) - ); - } - - ImPlot::EndPlot(); - } -} -} // namespace micras::sim - -#else // MICRAS_VIEWER - -namespace micras::sim { -PlotTrace::PlotTrace(std::string variable) : variable{std::move(variable)} { } - -void PlotTrace::sample(double /*time*/, const Telemetry& /*telemetry*/) { } - -ControlPanel::ControlPanel(GLFWwindow* /*window*/, ProxyState& state, const Telemetry& telemetry) : - state{state}, telemetry{telemetry} { } - -ControlPanel::~ControlPanel() = default; - -void ControlPanel::sample(double /*time*/) { } - -PanelRequest ControlPanel::draw(const Simulation& /*simulation*/, bool /*paused*/) { - return this->request; -} - -void ControlPanel::draw_board(bool /*paused*/) { } - -void ControlPanel::draw_plots() { } - -void ControlPanel::take_over() { } -} // namespace micras::sim - -#endif // MICRAS_VIEWER diff --git a/targets/toy/CMakeLists.txt b/targets/toy/CMakeLists.txt new file mode 100644 index 0000000..141e868 --- /dev/null +++ b/targets/toy/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################### +## Toy: the simulator's reference target, the smallest robot that uses every feature +############################################################################### + +add_library(micras_sim_toy STATIC src/toy_target.cpp) +target_include_directories(micras_sim_toy PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") +target_link_libraries(micras_sim_toy PUBLIC micras::sim_app) +target_compile_definitions(micras_sim_toy PRIVATE MICRAS_SIM_TOY_DIR="${CMAKE_CURRENT_SOURCE_DIR}") + +add_executable(toy_sim src/main.cpp) +target_link_libraries(toy_sim PRIVATE micras_sim_toy) + +micras_apply_warnings(micras_sim_toy toy_sim WERROR ${MICRAS_SIM_WERROR}) + +############################################################################### +## Checks: a run is reproducible whatever watches it, and matches its baseline +############################################################################### + +if(PROJECT_IS_TOP_LEVEL) + set(MICRAS_SIM_TOY_RUNS "${CMAKE_CURRENT_BINARY_DIR}/runs") + set(MICRAS_SIM_TOY_CHECKED idle drive) + + add_custom_target(micras_sim_toy_check + COMMAND "${CMAKE_COMMAND}" -E make_directory "${MICRAS_SIM_TOY_RUNS}" + COMMAND "${PROJECT_SOURCE_DIR}/scripts/check_invariance.sh" + "$" "${CMAKE_CURRENT_SOURCE_DIR}/scenarios/drive.toml" "${MICRAS_SIM_TOOLS_DIR}" + "${MICRAS_SIM_TOY_RUNS}/invariance" + COMMAND "${PROJECT_SOURCE_DIR}/scripts/check_baseline.sh" compare + "$" "${CMAKE_CURRENT_SOURCE_DIR}/scenarios" + "${CMAKE_CURRENT_SOURCE_DIR}/baseline" "${MICRAS_SIM_TOOLS_DIR}" + "${MICRAS_SIM_TOY_RUNS}" "${MICRAS_SIM_EXACT}" ${MICRAS_SIM_TOY_CHECKED} + COMMENT "Checking the toy target: invariance and baseline" + VERBATIM USES_TERMINAL + ) + add_dependencies(micras_sim_toy_check toy_sim) + + add_custom_target(micras_sim_toy_record_baseline + COMMAND "${CMAKE_COMMAND}" -E make_directory "${MICRAS_SIM_TOY_RUNS}" + COMMAND "${PROJECT_SOURCE_DIR}/scripts/check_baseline.sh" record + "$" "${CMAKE_CURRENT_SOURCE_DIR}/scenarios" + "${CMAKE_CURRENT_SOURCE_DIR}/baseline" "${MICRAS_SIM_TOOLS_DIR}" + "${MICRAS_SIM_TOY_RUNS}" OFF ${MICRAS_SIM_TOY_CHECKED} + COMMENT "Recording the toy's baseline" + VERBATIM USES_TERMINAL + ) + add_dependencies(micras_sim_toy_record_baseline toy_sim) +endif() diff --git a/targets/toy/baseline/drive/summary.json b/targets/toy/baseline/drive/summary.json new file mode 100644 index 0000000..471fa9d --- /dev/null +++ b/targets/toy/baseline/drive/summary.json @@ -0,0 +1,46 @@ +{ + "target": "toy", + "scenario": "drive.toml", + "args": "--scenario drive.toml --out drive", + "firmware_sha": "none", + "mujoco_version": "3.14.0", + "compiler": "GNU 15.2.0", + "data_sha256": "097ec972b9aa90d6ef8d1c66b2041258182ab56af70d52fa78ad926b75ad95cc", + "states": [ + [ + 0.001, + "WAIT" + ], + [ + 0.201, + "DRIVE" + ], + [ + 1.703, + "TURN" + ], + [ + 2.65, + "DRIVE" + ], + [ + 3.205, + "STOP" + ] + ], + "values": { + "ticks": 3205, + "warnings_total": 0, + "collisions": 0, + "goal_time": null, + "stopped_at": 3.205 + }, + "tolerances": { + "state_time": 0.25, + "ticks": null, + "warnings_total": null, + "collisions": 2, + "goal_time": 1.0, + "stopped_at": 1.0 + } +} diff --git a/targets/toy/baseline/idle/summary.json b/targets/toy/baseline/idle/summary.json new file mode 100644 index 0000000..5a4e152 --- /dev/null +++ b/targets/toy/baseline/idle/summary.json @@ -0,0 +1,30 @@ +{ + "target": "toy", + "scenario": "idle.toml", + "args": "--scenario idle.toml --out idle", + "firmware_sha": "none", + "mujoco_version": "3.14.0", + "compiler": "GNU 15.2.0", + "data_sha256": "b256361ea2dfacf35d25c66d63ca3b82d2fd5d3c633a557fb48b2d8e9a1b2d1a", + "states": [ + [ + 0.001, + "WAIT" + ] + ], + "values": { + "ticks": 2000, + "warnings_total": 0, + "collisions": 0, + "goal_time": null, + "stopped_at": null + }, + "tolerances": { + "state_time": 0.25, + "ticks": null, + "warnings_total": null, + "collisions": 2, + "goal_time": 1.0, + "stopped_at": 1.0 + } +} diff --git a/targets/toy/include/micras/sim/toy/toy_target.hpp b/targets/toy/include/micras/sim/toy/toy_target.hpp new file mode 100644 index 0000000..be58627 --- /dev/null +++ b/targets/toy/include/micras/sim/toy/toy_target.hpp @@ -0,0 +1,405 @@ +/** + * @file + * + * @brief The toy target: the smallest robot that exercises every feature of the engine and of Target. + */ + +#ifndef MICRAS_SIM_TOY_TOY_TARGET_HPP +#define MICRAS_SIM_TOY_TOY_TARGET_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/app/target.hpp" +#include "micras/sim/app/wiring.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/devices/dc_motor.hpp" +#include "micras/sim/devices/digital_input.hpp" +#include "micras/sim/devices/power.hpp" +#include "micras/sim/recording/column_source.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/recording/ground_truth.hpp" +#include "micras/sim/recording/run_metadata.hpp" + +namespace micras::sim { +/** + * @brief What the toy's program does. + */ +enum class ToyState : uint8_t { + WAIT, + DRIVE, + TURN, + STOP, +}; + +/** + * @brief The toy's hardware as its program sees it: what the devices wrote and what the program drives. + * + * @note The program and the devices never run at the same time, since the firmware thread hands over + * strictly, so this needs no lock. + */ +struct ToyBoard { + /** + * @brief Duty of each motor in percent, negative backwards. + */ + std::array motor_duty{}; + + /** + * @brief Whether the motor driver is enabled. + */ + bool motors_enabled{false}; + + /** + * @brief Duty of the fan in percent. + */ + float fan_duty{0.0F}; + + /** + * @brief Whether the wall sensors' emitters are on. + */ + bool emitters_on{false}; + + /** + * @brief Count of each wheel's encoder. + */ + std::array encoder_counts{}; + + /** + * @brief Last yaw rate the IMU delivered, in raw words. + */ + float gyro_z_word{0.0F}; + + /** + * @brief Wall sensor readings: each receiver with the first group lit, then with the second. + */ + std::array wall_counts{}; + + /** + * @brief Battery divider reading. + */ + uint32_t battery_counts{0}; + + /** + * @brief Motor current readings. + */ + std::array current_counts{}; + + /** + * @brief Level of the button's pin, high when pressed. + */ + bool button_level{false}; + + /** + * @brief Bytes the link delivered, not read yet. + */ + std::deque received; + + /** + * @brief Bytes the program sent, not on the link yet. + */ + std::deque to_send; +}; + +/** + * @brief What the toy's program knows, recorded as columns and read by the panel and the scenarios. + */ +struct ToyReadings { + /** + * @brief State of the program. + */ + ToyState state{ToyState::WAIT}; + + /** + * @brief Distance the wheels rolled, in meters. + */ + double odometry{0.0}; + + /** + * @brief Heading integrated from the gyro, in radians. + */ + double heading{0.0}; + + /** + * @brief Front wall sensor reading, with its own emitter lit, in counts. + */ + double front{0.0}; + + /** + * @brief Pack voltage, in volts. + */ + double voltage{0.0}; + + /** + * @brief Current of each motor, in amperes. + */ + std::array currents{}; +}; + +/** + * @brief The toy's variables: CSV columns and what scenarios and the overlay read. + */ +class ToyVariables : public ColumnSource, public VariableSource { +public: + /** + * @brief Read from the program's readings. + * + * @param readings The readings, which outlive the variables. + */ + explicit ToyVariables(const ToyReadings& readings); + + /** + * @brief Get the names of the columns. + * + * @return The names, in the order append() writes them. + */ + std::vector names() override; + + /** + * @brief Append the current readings to a row. + * + * @param row Row to append to. + */ + void append(std::vector& row) override; + + /** + * @brief Get a variable by name. + * + * @param name Name of a column. + * @return Its value. + */ + double value_of(const std::string& name) const override; + +private: + /** + * @brief The program's readings. + */ + const ToyReadings& readings; // NOLINT(*-avoid-const-or-ref-data-members): the target owns both. +}; + +/** + * @brief The toy robot: drives until a wall, turns, drives on, and stops when told. + * + * @note No HAL, nothing of a real robot: the program runs on the firmware thread and yields a tick + * itself once per loop, and the devices write straight into a ToyBoard. + */ +class ToyTarget : public Target { +public: + /** + * @brief Get the name of the robot. + * + * @return "toy", which its scenarios name. + */ + std::string name() const override; + + /** + * @brief Get the firmware commit. + * + * @return "none": the toy's program is part of the simulator. + */ + std::string firmware_sha() const override; + + /** + * @brief Get the loop period of the program. + * + * @return 1000 us. + */ + uint32_t loop_time_us() const override; + + /** + * @brief Get the toy's own option, --turn-angle. + * + * @return The options. + */ + std::vector options() override; + + /** + * @brief Get the context of the run. + * + * @return This target's context. + */ + RunContext& context() override; + + /** + * @brief Get the folder of this target. + * + * @return Path of targets/toy. + */ + std::filesystem::path directory() const override; + + /** + * @brief Get the robot's physical description. + * + * @return Path of targets/toy/robot.toml. + */ + std::filesystem::path robot_file() const override; + + /** + * @brief Get the ground truth columns of a toy run. + * + * @return The recorder configuration. + */ + GroundTruthConfig ground_truth() const override; + + /** + * @brief Get the camera of a video. + * + * @return The camera above the robot. + */ + std::string video_camera() const override; + + /** + * @brief Get the program, which yields on the thread it is given once per loop. + * + * @return The program. + */ + FirmwareThread::Program program() override; + + /** + * @brief Build the toy's devices, columns, panel and scenario hooks. + * + * @param firmware Thread that runs the program. + * @param world The robot and the arena's surfaces. + * @return What the toy adds to the run. + */ + Wiring wire(FirmwareThread& firmware, const WorldInfo& world) override; + + /** + * @brief Get the toy's counters for meta.json. + * + * @return The bytes it sent over the link. + */ + std::vector metadata() const override; + +private: + /** + * @brief Run the program: one loop per tick, forever. + * + * @param thread Thread the program runs on, which it yields. + */ + void run(FirmwareThread& thread); + + /** + * @brief Update the readings from what the devices wrote. + * + * @param dt Time since the last loop, in seconds. + */ + void read_sensors(double dt); + + /** + * @brief Take the bytes that arrived over the link. + * + * @return "go" or "stop" when the bytes just completed one, else empty. + */ + std::string read_link(); + + /** + * @brief Send text over the link. + * + * @param text Bytes to send. + */ + void send(const std::string& text); + + /** + * @brief Drive the motors, the fan and the emitters for the current state. + */ + void actuate(); + + /** + * @brief Build the panel. + * + * @return The button, a lamp per state and the pack voltage. + */ + PanelSpec make_panel() const; + + /** + * @brief Build the scenario hooks. + * + * @return The button, the go and stop messages and the state names. + */ + ScenarioHooks make_hooks() const; + + /** + * @brief The run's context. + */ + RunContext run_context; + + /** + * @brief The toy's hardware. + */ + ToyBoard board; + + /** + * @brief What the program knows. + */ + ToyReadings readings; + + /** + * @brief The recorded variables. + */ + ToyVariables variables{readings}; + + /** + * @brief The button's input device. + */ + DigitalInput* button{nullptr}; + + /** + * @brief The battery, for the panel. + */ + const Battery* battery{nullptr}; + + /** + * @brief Heading change the program turns at a wall, in radians, positive to the right. + */ + double turn_angle{std::numbers::pi / 2.0}; + + /** + * @brief Heading when the current turn started, in radians. + */ + double turn_start{0.0}; + + /** + * @brief Radius of the wheels, from the robot's description. + */ + double wheel_radius{0.0}; + + /** + * @brief Encoder counts per wheel revolution, from the robot's description. + */ + double counts_per_revolution{1.0}; + + /** + * @brief Gyro resolution, from the robot's description, in rad/s per word. + */ + double gyro_resolution{0.0}; + + /** + * @brief Divider of the battery reading. + */ + double battery_divider{3.0}; + + /** + * @brief Bytes of the last command, to recognize "go" and "stop" in the raw byte stream. + */ + std::string command; + + /** + * @brief Whether the button was pressed on the previous loop, to start on the press. + */ + bool was_pressed{false}; + + /** + * @brief Bytes the program sent over the link. + */ + int64_t bytes_sent{0}; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_TOY_TOY_TARGET_HPP diff --git a/targets/toy/robot.toml b/targets/toy/robot.toml new file mode 100644 index 0000000..ca0ad18 --- /dev/null +++ b/targets/toy/robot.toml @@ -0,0 +1,113 @@ +# The toy target's robot: the engine tests' tiny robot (tests/models/tiny_robot.toml), +# a made-up two-wheeled robot with every key the schema requires, with its own name, +# a timestep of 125 us that divides the toy's 1 ms loop, and a second wall sensor +# looking ahead, since the wall sensors fire in two groups. + +schema = 1 +name = "toy" + +[integration] +timestep = { value = 0.000125, source = "divides the 1 ms loop into eight steps" } + +[chassis] +mass = { value = 0.05, source = "made up" } +center_of_mass = [0.0, 0.0, 0.01] +inertia = [1.0e-5, 1.5e-5, 2.0e-5] +outline = [[0.04, 0.02], [-0.03, 0.02], [-0.03, -0.02], [0.04, -0.02]] +board_bottom = 0.002 +board_top = 0.003 +friction = 0.3 + +[[chassis.boxes]] +name = "cargo" +min = [-0.02, -0.015, 0.003] +max = [0.02, 0.015, 0.02] + +[[chassis.cylinders]] +name = "mast" +base = [0.0, 0.0, 0.02] +radius = 0.005 +height = 0.01 + +[wheels] +radius = 0.011 +width = 0.004 +track = 0.05 +mass = 0.004 +spin_inertia = 2.0e-7 +friction = 1.0 +torsional_friction = 0.0005 +contact_time_constant = 0.008 +contact_damping_ratio = 1.0 + +[drive] +winding_resistance = { value = 10.0, source = "made up" } +bridge_resistance = 0.5 +torque_constant = 0.003 +speed_constant = 0.003 +no_load_current = 0.007 +rotor_inertia = 4.0e-9 +gear_ratio = 5.0 +gear_efficiency = 0.9 +supply_voltage = 6.0 + +[encoders] +counts_per_revolution = { value = 4096, source = "made up" } + +[imu] +position = [0.0, 0.0, 0.004] +chip_x = [1.0, 0.0, 0.0] +chip_y = [0.0, 1.0, 0.0] +gyro_noise_density = 5.0e-5 +gyro_bias = 0.005 +gyro_scale_error = 0.001 +gyro_resolution = 0.0025 +accel_noise_density = 6.0e-4 +accel_bias = 0.04 +accel_resolution = 0.0024 +bandwidth = 250.0 +output_rate = 1000.0 +clock_error = 0.005 + +[fan] +position = [0.0, 0.0, 0.003] +max_downforce = 0.1 +nominal_voltage = 7.4 +time_constant = 0.1 + +[battery] +cells = 2 +cell_voltage = 3.7 + +[link] +baud_rate = 9600 + +[wall_sensors] +emitter_half_angle_deg = 5.0 +emitter_intensity = 1.0 +receiver_half_angle_deg = 10.0 +receiver_responsivity = 4.0e-4 +load_resistance = 1000.0 +saturation_voltage = 3.0 +adc_reference = 3.3 +adc_max_counts = 4095.0 +adc_noise_counts = 2.0 +settle_fraction = 0.95 +ambient_irradiance = 0.01 +rays = 4 + +[[wall_sensors.sensors]] +name = "eye" +emitter = [0.04, 0.0, 0.01] +receiver = [0.04, 0.005, 0.01] +yaw_deg = { value = 90.0, source = "made up, to check the conversion" } +group = 0 +gain = 1.0 + +[[wall_sensors.sensors]] +name = "front" +emitter = [0.04, -0.005, 0.01] +receiver = [0.04, 0.0, 0.01] +yaw_deg = 0.0 +group = 1 +gain = 1.0 diff --git a/targets/toy/scenarios/drive.toml b/targets/toy/scenarios/drive.toml new file mode 100644 index 0000000..de7809f --- /dev/null +++ b/targets/toy/scenarios/drive.toml @@ -0,0 +1,25 @@ +# A press of the button starts the toy: it drives up the first corridor, turns right +# at the wall ahead, drives on, and stops when the link tells it to. +robot = "toy" +arena = "maze1" +seconds = 10 +seed = 1 + +[start] +x = 0.09 +y = 0.05 +yaw_deg = 90.0 + +[[events]] +at = 0.2 +press = "button" +for = 0.1 + +[[events]] +at = 3.2 +send = "stop" + +[stop] +when = "state" +equals = ["STOP"] +after = 0.5 diff --git a/targets/toy/scenarios/idle.toml b/targets/toy/scenarios/idle.toml new file mode 100644 index 0000000..b2f6b5b --- /dev/null +++ b/targets/toy/scenarios/idle.toml @@ -0,0 +1,10 @@ +# The toy switched on in the start cell and left alone: it waits. +robot = "toy" +arena = "maze1" +seconds = 2 +seed = 1 + +[start] +x = 0.09 +y = 0.05 +yaw_deg = 90.0 diff --git a/targets/toy/src/main.cpp b/targets/toy/src/main.cpp new file mode 100644 index 0000000..b6eb2bf --- /dev/null +++ b/targets/toy/src/main.cpp @@ -0,0 +1,14 @@ +/** + * @file + */ + +#include +#include + +#include "micras/sim/app/application.hpp" +#include "micras/sim/toy/toy_target.hpp" + +int main(int argc, char** argv) { + micras::sim::ToyTarget target; + return micras::sim::run(std::span(argv, static_cast(argc)), target); +} diff --git a/targets/toy/src/toy_target.cpp b/targets/toy/src/toy_target.cpp new file mode 100644 index 0000000..7a93335 --- /dev/null +++ b/targets/toy/src/toy_target.cpp @@ -0,0 +1,495 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/app/wiring.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/devices/dc_motor.hpp" +#include "micras/sim/devices/digital_input.hpp" +#include "micras/sim/devices/imu.hpp" +#include "micras/sim/devices/power.hpp" +#include "micras/sim/devices/quadrature_encoder.hpp" +#include "micras/sim/devices/serial_link.hpp" +#include "micras/sim/devices/wall_sensors.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/recording/ground_truth.hpp" +#include "micras/sim/recording/run_metadata.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/robot/robot_model.hpp" +#include "micras/sim/scenario/scenario.hpp" +#include "micras/sim/toy/toy_target.hpp" +#include "micras/sim/view/panel_spec.hpp" + +namespace micras::sim { +namespace { +constexpr std::array state_name_table{"WAIT", "DRIVE", "TURN", "STOP"}; + +constexpr uint32_t loop_period_us{1000}; + +constexpr float drive_duty{12.0F}; + +constexpr float turn_duty{6.0F}; + +constexpr float fan_duty{50.0F}; + +constexpr double front_threshold{400.0}; + +constexpr std::size_t front_reading{3}; + +constexpr double adc_reference{3.3}; + +constexpr double adc_max_counts{4095.0}; + +constexpr double current_zero_voltage{1.65}; + +constexpr double volts_per_amp{0.5}; +} // namespace + +static std::vector state_names() { + return {state_name_table.begin(), state_name_table.end()}; +} + +template +static T* add(RunContext& context, std::unique_ptr device) { + T* view = device.get(); + context.devices.push_back(std::move(device)); + return view; +} + +ToyVariables::ToyVariables(const ToyReadings& readings) : readings{readings} { } + +std::vector ToyVariables::names() { + return {"state", "odometry", "heading", "front", "voltage", "current_left", "current_right"}; +} + +void ToyVariables::append(std::vector& row) { + row.emplace_back(static_cast(std::to_underlying(this->readings.state))); + row.emplace_back(this->readings.odometry); + row.emplace_back(this->readings.heading); + row.emplace_back(this->readings.front); + row.emplace_back(this->readings.voltage); + row.emplace_back(this->readings.currents.at(0)); + row.emplace_back(this->readings.currents.at(1)); +} + +double ToyVariables::value_of(const std::string& name) const { + if (name == "state") { + return static_cast(std::to_underlying(this->readings.state)); + } + + if (name == "odometry") { + return this->readings.odometry; + } + + if (name == "heading") { + return this->readings.heading; + } + + if (name == "front") { + return this->readings.front; + } + + if (name == "voltage") { + return this->readings.voltage; + } + + if (name == "current_left") { + return this->readings.currents.at(0); + } + + if (name == "current_right") { + return this->readings.currents.at(1); + } + + throw std::runtime_error("the toy has no variable named " + name); +} + +std::string ToyTarget::name() const { + return "toy"; +} + +std::string ToyTarget::firmware_sha() const { + return "none"; +} + +uint32_t ToyTarget::loop_time_us() const { + return loop_period_us; +} + +std::vector ToyTarget::options() { + return {{ + .name = "--turn-angle", + .argument = "", + .apply = [this](const std::string& value) { + double degrees = 0.0; + const char* const end = std::to_address(value.end()); + const auto result = std::from_chars(value.data(), end, degrees); + + if (result.ec != std::errc{} or result.ptr != end or degrees <= 0.0) { + throw std::runtime_error("--turn-angle expects a positive number of degrees, got '" + value + "'"); + } + + this->turn_angle = degrees * std::numbers::pi / 180.0; + }, + }}; +} + +RunContext& ToyTarget::context() { + return this->run_context; +} + +std::filesystem::path ToyTarget::directory() const { + return std::filesystem::path{MICRAS_SIM_TOY_DIR}; +} + +std::filesystem::path ToyTarget::robot_file() const { + return this->directory() / "robot.toml"; +} + +GroundTruthConfig ToyTarget::ground_truth() const { + return { + .body = "toy", + .columns = { + {.name = "wheel_speed_left", .probe = Probe::JOINT_VELOCITY, .object = "left_wheel"}, + {.name = "wheel_speed_right", .probe = Probe::JOINT_VELOCITY, .object = "right_wheel"}, + {.name = "motor_torque_left", .probe = Probe::ACTUATOR_FORCE, .object = "motor_left"}, + {.name = "motor_torque_right", .probe = Probe::ACTUATOR_FORCE, .object = "motor_right"}, + {.name = "left_ncon", .probe = Probe::CONTACT_COUNT, .object = "left_wheel"}, + {.name = "left_slip", .probe = Probe::CONTACT_SLIP, .object = "left_wheel"}, + {.name = "right_ncon", .probe = Probe::CONTACT_COUNT, .object = "right_wheel"}, + {.name = "right_slip", .probe = Probe::CONTACT_SLIP, .object = "right_wheel"}, + {.name = "solver_niter", .probe = Probe::SOLVER_ITERATIONS, .object = ""}, + }, + }; +} + +std::string ToyTarget::video_camera() const { + return "top tracking"; +} + +FirmwareThread::Program ToyTarget::program() { + return [this](FirmwareThread& thread) { this->run(thread); }; +} + +Wiring ToyTarget::wire(FirmwareThread& /*firmware*/, const WorldInfo& world) { + const RobotDescription& robot = *world.robot; + const RobotModelNames names = RobotModelNames::of(robot); + RunContext& context = this->run_context; + ToyBoard& hardware = this->board; + + this->wheel_radius = robot.wheels.radius; + this->counts_per_revolution = static_cast(robot.encoders.counts_per_revolution); + this->gyro_resolution = robot.imu.gyro_resolution; + + const std::array sides{"left", "right"}; + std::array motors{}; + + for (std::size_t index = 0; index < sides.size(); index++) { + const std::string& side = sides.at(index); + + motors.at(index) = + add(context, + std::make_unique( + context.world, + DcMotor::Config{ + .name = "motor_" + side, + .actuator = index == 0 ? names.left_motor : names.right_motor, + .joint = index == 0 ? names.left_wheel : names.right_wheel, + .drive = robot.drive, + .forward_duty = [&hardware, index] { return std::max(hardware.motor_duty.at(index), 0.0F); }, + .backward_duty = [&hardware, index] { return std::max(-hardware.motor_duty.at(index), 0.0F); }, + .enabled = [&hardware] { return hardware.motors_enabled; }, + } + )); + + add(context, std::make_unique( + context.world, + QuadratureEncoder::Config{ + .name = "encoder_" + side, + .joint = index == 0 ? names.left_wheel : names.right_wheel, + .counts_per_revolution = robot.encoders.counts_per_revolution, + .write = [&hardware, index](int32_t count) { hardware.encoder_counts.at(index) = count; }, + } + )); + } + + add(context, std::make_unique( + context.world, + Imu::Config{ + .name = "imu", + .gyro = names.gyro, + .accelerometer = names.accelerometer, + .description = robot.imu, + .write = [&hardware](std::span sample) { hardware.gyro_z_word = at(sample, 2); }, + }, + context.noise + )); + + add(context, + std::make_unique( + context.world, + WallSensors::Config{ + .name = "wall", + .description = robot.wall_sensors, + .scan_ticks = 1, + .emitter_duty = [&hardware](std::size_t /*sensor*/) { return hardware.emitters_on ? 100.0F : 0.0F; }, + .write = [&hardware](std::size_t index, uint32_t counts) { hardware.wall_counts.at(index) = counts; }, + .finish_sequence = [] { }, + .reflectance = world.reflectance, + }, + context.noise + )); + + const Battery* pack = + add(context, std::make_unique( + Battery::Config{ + .name = "pack", + .description = robot.battery, + .divider = this->battery_divider, + .adc_reference = adc_reference, + .adc_max_counts = adc_max_counts, + .adc_noise_counts = 1.0, + .write = [&hardware](uint32_t counts) { hardware.battery_counts = counts; }, + }, + context.noise + )); + this->battery = pack; + + add(context, std::make_unique( + context.world, Fan::Config{ + .name = "fan", + .actuator = names.fan, + .description = robot.fan, + .duty = [&hardware] { return hardware.fan_duty; }, + .enabled = [&hardware] { return hardware.fan_duty > 0.0F; }, + .supply_voltage = [pack] { return pack->voltage(); }, + } + )); + + add(context, + std::make_unique( + CurrentSense::Config{ + .currents = { + [motors] { return motors.at(0)->current(); }, [motors] { return motors.at(1)->current(); } + }, + .zero_voltage = current_zero_voltage, + .volts_per_amp = volts_per_amp, + .adc_reference = adc_reference, + .adc_max_counts = adc_max_counts, + .adc_noise_counts = 2.0, + .write = + [&hardware](std::size_t index, uint32_t counts) { hardware.current_counts.at(index) = counts; }, + }, + context.noise + )); + + add(context, std::make_unique( + context.serial, SerialLink::Config{ + .baud_rate = robot.link.baud_rate, + .take_sent = [&hardware]() -> std::optional { + if (hardware.to_send.empty()) { + return std::nullopt; + } + + const uint8_t byte = hardware.to_send.front(); + hardware.to_send.pop_front(); + return byte; + }, + .receive = [&hardware](uint8_t byte) { hardware.received.push_back(byte); }, + } + )); + + this->button = + add(context, std::make_unique(DigitalInput::Config{ + .name = "button", + .active_low = false, + .drive = [&hardware](bool level) { hardware.button_level = level; }, + })); + + return { + .columns = {&this->variables}, + .variables = &this->variables, + .panel = this->make_panel(), + .overlay = + {.state = StateLabel{.variable = "state", .names = state_names()}, + .lines = {{.label = "odometry", .variable = "odometry", .unit = "m"}}}, + .hooks = this->make_hooks(), + }; +} + +std::vector ToyTarget::metadata() const { + return {{.name = "bytes_sent", .value = this->bytes_sent}}; +} + +void ToyTarget::run(FirmwareThread& thread) { + const double dt = static_cast(loop_period_us) * 1e-6; + + while (true) { + this->read_sensors(dt); + + const std::string command = this->read_link(); + const bool pressed = this->board.button_level; + const bool press = pressed and not this->was_pressed; + this->was_pressed = pressed; + + switch (this->readings.state) { + case ToyState::WAIT: + if (press or command == "go") { + this->readings.state = ToyState::DRIVE; + this->send("driving\n"); + } + + break; + case ToyState::DRIVE: + if (this->readings.front >= front_threshold) { + this->readings.state = ToyState::TURN; + this->turn_start = this->readings.heading; + } + + break; + case ToyState::TURN: + if (this->turn_start - this->readings.heading >= this->turn_angle) { + this->readings.state = ToyState::DRIVE; + } + + break; + case ToyState::STOP: + break; + } + + if (command == "stop" and this->readings.state != ToyState::STOP) { + this->readings.state = ToyState::STOP; + this->send("stopped\n"); + } + + this->actuate(); + thread.yield_tick(); + } +} + +void ToyTarget::read_sensors(double dt) { + const std::array& counts = this->board.encoder_counts; + const double revolutions = static_cast(counts.at(0) + counts.at(1)) / (2.0 * this->counts_per_revolution); + + this->readings.odometry = revolutions * 2.0 * std::numbers::pi * this->wheel_radius; + + if (this->readings.state == ToyState::TURN) { + this->readings.heading += static_cast(this->board.gyro_z_word) * this->gyro_resolution * dt; + } + + this->readings.front = static_cast(this->board.wall_counts.at(front_reading)); + this->readings.voltage = + static_cast(this->board.battery_counts) / adc_max_counts * adc_reference * this->battery_divider; + + for (std::size_t index = 0; index < this->readings.currents.size(); index++) { + const double volts = static_cast(this->board.current_counts.at(index)) / adc_max_counts * adc_reference; + this->readings.currents.at(index) = (volts - current_zero_voltage) / volts_per_amp; + } +} + +std::string ToyTarget::read_link() { + std::string completed; + + while (not this->board.received.empty()) { + this->command.push_back(static_cast(this->board.received.front())); + this->board.received.pop_front(); + + if (this->command.ends_with("go")) { + completed = "go"; + this->command.clear(); + } else if (this->command.ends_with("stop")) { + completed = "stop"; + this->command.clear(); + } else if (this->command.size() > 4) { + this->command.erase(0, this->command.size() - 4); + } + } + + return completed; +} + +void ToyTarget::send(const std::string& text) { + for (const char character : text) { + this->board.to_send.push_back(static_cast(character)); + } + + this->bytes_sent += static_cast(text.size()); +} + +void ToyTarget::actuate() { + ToyBoard& hardware = this->board; + hardware.emitters_on = true; + + switch (this->readings.state) { + case ToyState::DRIVE: + hardware.motor_duty = {drive_duty, drive_duty}; + hardware.motors_enabled = true; + hardware.fan_duty = fan_duty; + break; + case ToyState::TURN: + hardware.motor_duty = {turn_duty, -turn_duty}; + hardware.motors_enabled = true; + hardware.fan_duty = fan_duty; + break; + case ToyState::WAIT: + case ToyState::STOP: + hardware.motor_duty = {0.0F, 0.0F}; + hardware.motors_enabled = false; + hardware.fan_duty = 0.0F; + break; + } +} + +PanelSpec ToyTarget::make_panel() const { + DigitalInput* input = this->button; + const Battery* pack = this->battery; + const ToyReadings& known = this->readings; + PanelSpec panel{ + .state = StateLabel{.variable = "state", .names = state_names()}, + .buttons = {{.name = "button", .press = [input](bool pressed) { input->set(pressed); }}}, + .switches = {}, + .lamps = {}, + .readouts = {{.name = "battery", .text = [pack] { return std::format("{:.2f} V", pack->voltage()); }}}, + .plots = {"odometry", "heading", "front"}, + .take_over = {}, + }; + + for (std::size_t index = 0; index < state_name_table.size(); index++) { + panel.lamps.push_back({.name = state_name_table.at(index), .color = [&known, index] { + return std::to_underlying(known.state) == index ? + Color{.red = 60, .green = 220, .blue = 60} : + Color{.red = 40, .green = 40, .blue = 40}; + }}); + } + + return panel; +} + +ScenarioHooks ToyTarget::make_hooks() const { + ScenarioHooks hooks; + hooks.inputs.emplace("button", this->button); + hooks.messages.emplace("go", std::vector{'g', 'o'}); + hooks.messages.emplace("stop", std::vector{'s', 't', 'o', 'p'}); + hooks.state_names.emplace("state", state_names()); + return hooks; +} +} // namespace micras::sim diff --git a/tests/.clang-tidy b/tests/.clang-tidy index f3c0b81..ce6b632 100644 --- a/tests/.clang-tidy +++ b/tests/.clang-tidy @@ -1,6 +1,6 @@ --- -# The GoogleTest macros expand into try/catch blocks, so the cognitive -# complexity of a TEST body measures the macro rather than the test. +# The test macros expand into branches that count against the cognitive complexity of every test +# case, whatever the test itself does InheritParentConfig: true Checks: -readability-function-cognitive-complexity, diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ae922eb..656f804 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,35 +1,36 @@ ############################################################################### -## Unit tests (GoogleTest + CTest) +## Engine unit tests (doctest + CTest), on a tiny robot of their own ############################################################################### -include(GoogleTest) - add_executable(micras_sim_tests unit/cli_test.cpp unit/clock_test.cpp + unit/csv_recorder_test.cpp + unit/csv_writer_test.cpp + unit/devices_test.cpp + unit/event_log_test.cpp unit/firmware_thread_test.cpp - unit/simulation_test.cpp + unit/maze_test.cpp + unit/mujoco_world_test.cpp + unit/noise_test.cpp + unit/robot_description_test.cpp + unit/run_metadata_test.cpp unit/scenario_test.cpp unit/serial_bus_test.cpp - unit/packet_framer_test.cpp - unit/telemetry_test.cpp - unit/csv_writer_test.cpp - unit/csv_recorder_test.cpp - unit/run_metadata_test.cpp - unit/proxy_boundary_test.cpp - unit/shadow_conformance_test.cpp + unit/simulation_test.cpp ) -target_link_libraries(micras_sim_tests PRIVATE micras_sim_app GTest::gtest_main) -target_compile_options(micras_sim_tests PRIVATE -Wall -Wextra) - -# The conformance test initialises only the fields it shares with the firmware. -set_source_files_properties(unit/shadow_conformance_test.cpp - PROPERTIES COMPILE_OPTIONS -Wno-missing-field-initializers -) +# The bridge tests need the real server, which -DMICRAS_SIM_BRIDGE=OFF replaces by a stub. +if(MICRAS_SIM_BRIDGE) + target_sources(micras_sim_tests PRIVATE unit/web_socket_server_test.cpp) +endif() +target_link_libraries(micras_sim_tests PRIVATE micras::sim_app doctest::doctest_with_main) +micras_apply_warnings(micras_sim_tests WERROR ${MICRAS_SIM_WERROR}) target_compile_definitions(micras_sim_tests PRIVATE - MICRAS_TEST_MODEL="${CMAKE_SOURCE_DIR}/models/robot_v2.xml" + MICRAS_TEST_MODEL="${CMAKE_CURRENT_SOURCE_DIR}/models/tiny_robot.xml" + MICRAS_TEST_ROBOT="${CMAKE_CURRENT_SOURCE_DIR}/models/tiny_robot.toml" + MICRAS_TEST_MAZES_DIR="${PROJECT_SOURCE_DIR}/engine/arenas/maze/mazes" ) -gtest_discover_tests(micras_sim_tests) +doctest_discover_tests(micras_sim_tests) diff --git a/tests/include/test_core.hpp b/tests/include/test_core.hpp deleted file mode 100644 index 3acc92d..0000000 --- a/tests/include/test_core.hpp +++ /dev/null @@ -1,69 +0,0 @@ -/** - * @file - * - * @brief Entry points a firmware hardware test is written against. - */ - -#ifndef MICRAS_TEST_CORE_HPP -#define MICRAS_TEST_CORE_HPP - -#include - -#include "micras/sim/core/simulation_context.hpp" -#include "target.hpp" - -namespace micras { -template -concept VoidFunction = requires(F void_function) { - { void_function() } -> std::same_as; -}; - -/** - * @brief Core class to the tests, backed by the simulation instead of the MCU. - * - * @note The hardware tests are compiled unchanged: their main is renamed and - * run inside the firmware thread, so by the time init() is reached the - * model is loaded and the proxies can be built. There is nothing left for - * init() to do. - * - * @note The loop hands a tick over only when the iteration did not already do - * so, so a test that waits on a stopwatch keeps its own pace and a test - * that never looks at the clock still lets the world advance. - */ -class TestCore { -public: - /** - * @brief Delete the default constructor. - */ - TestCore() = delete; - - /** - * @brief Initialize the test core. - * - * @param argc Number of main arguments. - * @param argv Main arguments. - */ - static void init(int /*argc*/ = 0, char** /*argv*/ = nullptr) { } - - /** - * @brief Loop the test core with a custom function. - * - * @param loop_func Custom loop function. - */ - static void loop(VoidFunction auto loop_func) { - const sim::Clock& clock = sim::SimulationContext::instance().clock; - - while (true) { - const uint64_t started_at = clock.tick_count(); - - loop_func(); - - if (clock.tick_count() == started_at) { - sim::yield_tick(); - } - } - } -}; -} // namespace micras - -#endif // MICRAS_TEST_CORE_HPP diff --git a/tests/models/tiny_robot.toml b/tests/models/tiny_robot.toml new file mode 100644 index 0000000..57d3256 --- /dev/null +++ b/tests/models/tiny_robot.toml @@ -0,0 +1,104 @@ +# The smallest robot description the engine tests need: a made-up two-wheeled +# robot with every key the schema requires and nothing else. Some values name a +# source and some do not, because the file format accepts both. + +schema = 1 +name = "tiny" + +[integration] +timestep = { value = 0.000521, source = "divides a 1042 us loop into two steps" } + +[chassis] +mass = { value = 0.05, source = "made up" } +center_of_mass = [0.0, 0.0, 0.01] +inertia = [1.0e-5, 1.5e-5, 2.0e-5] +outline = [[0.04, 0.02], [-0.03, 0.02], [-0.03, -0.02], [0.04, -0.02]] +board_bottom = 0.002 +board_top = 0.003 +friction = 0.3 + +[[chassis.boxes]] +name = "cargo" +min = [-0.02, -0.015, 0.003] +max = [0.02, 0.015, 0.02] + +[[chassis.cylinders]] +name = "mast" +base = [0.0, 0.0, 0.02] +radius = 0.005 +height = 0.01 + +[wheels] +radius = 0.011 +width = 0.004 +track = 0.05 +mass = 0.004 +spin_inertia = 2.0e-7 +friction = 1.0 +torsional_friction = 0.0005 +contact_time_constant = 0.008 +contact_damping_ratio = 1.0 + +[drive] +winding_resistance = { value = 10.0, source = "made up" } +bridge_resistance = 0.5 +torque_constant = 0.003 +speed_constant = 0.003 +no_load_current = 0.007 +rotor_inertia = 4.0e-9 +gear_ratio = 5.0 +gear_efficiency = 0.9 +supply_voltage = 6.0 + +[encoders] +counts_per_revolution = { value = 4096, source = "made up" } + +[imu] +position = [0.0, 0.0, 0.004] +chip_x = [1.0, 0.0, 0.0] +chip_y = [0.0, 1.0, 0.0] +gyro_noise_density = 5.0e-5 +gyro_bias = 0.005 +gyro_scale_error = 0.001 +gyro_resolution = 0.0025 +accel_noise_density = 6.0e-4 +accel_bias = 0.04 +accel_resolution = 0.0024 +bandwidth = 250.0 +output_rate = 1000.0 +clock_error = 0.005 + +[fan] +position = [0.0, 0.0, 0.003] +max_downforce = 0.1 +nominal_voltage = 7.4 +time_constant = 0.1 + +[battery] +cells = 2 +cell_voltage = 3.7 + +[link] +baud_rate = 9600 + +[wall_sensors] +emitter_half_angle_deg = 5.0 +emitter_intensity = 1.0 +receiver_half_angle_deg = 10.0 +receiver_responsivity = 4.0e-4 +load_resistance = 1000.0 +saturation_voltage = 3.0 +adc_reference = 3.3 +adc_max_counts = 4095.0 +adc_noise_counts = 2.0 +settle_fraction = 0.95 +ambient_irradiance = 0.01 +rays = 4 + +[[wall_sensors.sensors]] +name = "eye" +emitter = [0.04, 0.0, 0.01] +receiver = [0.04, 0.005, 0.01] +yaw_deg = { value = 90.0, source = "made up, to check the conversion" } +group = 0 +gain = 1.0 diff --git a/tests/models/tiny_robot.xml b/tests/models/tiny_robot.xml new file mode 100644 index 0000000..d0bd245 --- /dev/null +++ b/tests/models/tiny_robot.xml @@ -0,0 +1,25 @@ + + + diff --git a/tests/unit/cli_test.cpp b/tests/unit/cli_test.cpp index 4bd9935..1b7d0a7 100644 --- a/tests/unit/cli_test.cpp +++ b/tests/unit/cli_test.cpp @@ -1,24 +1,23 @@ -#include #include +#include #include #include -#include +#include #include "micras/sim/app/cli.hpp" +#include "micras/sim/app/wiring.hpp" namespace micras::sim { -namespace { -constexpr uint32_t firmware_loop_us{1042}; - /** * @brief Parse a command line written the way a shell would pass it. * * @param words Arguments after the program name. + * @param target_options Options the robot target adds. * @return Parsed options. */ -CliOptions parse(std::vector words) { - std::vector owned{"micras_simulation"}; +static CliOptions parse(std::vector words, std::span target_options = {}) { + std::vector owned{"robot_sim"}; owned.insert(owned.end(), words.begin(), words.end()); std::vector argv; @@ -28,102 +27,109 @@ CliOptions parse(std::vector words) { argv.push_back(word.data()); } - return Cli::parse(std::span(argv), firmware_loop_us); + return Cli::parse(std::span(argv), target_options); } -TEST(Cli, ReadsTheRequiredOptions) { - const CliOptions options = parse({"--model", "robot.xml", "--out", "runs/here"}); - - EXPECT_EQ(options.model, "robot.xml"); - EXPECT_EQ(options.out, "runs/here"); - EXPECT_EQ(options.scenario.command, Command::NONE); - EXPECT_EQ(options.scenario.button, ButtonPress::NONE); - EXPECT_TRUE(options.scenario.fan_enabled); - EXPECT_FALSE(options.video_enabled); -} - -TEST(Cli, TurnsSecondsIntoTheTickCountEveryRunUses) { - EXPECT_EQ(parse({"--model", "m.xml", "--out", "o", "--seconds", "4"}).ticks, 3838U); - EXPECT_EQ(parse({"--model", "m.xml", "--out", "o", "--seconds", "8"}).ticks, 7677U); - EXPECT_EQ(parse({"--model", "m.xml", "--out", "o", "--ticks", "12"}).ticks, 12U); +namespace { +TEST_CASE("Cli.ReadsTheRequiredOptions") { + const CliOptions options = parse({"--out", "runs/here"}); + + CHECK_EQ(options.out, "runs/here"); + CHECK(options.scenario.empty()); + CHECK_FALSE(options.seconds.has_value()); + CHECK_FALSE(options.seed.has_value()); + CHECK_FALSE(options.ideal); + CHECK_EQ(options.record_every, 1U); + CHECK(options.video.path.empty()); + CHECK(options.video.camera.empty()); + CHECK_FALSE(options.viewer_enabled); } -TEST(Cli, ReadsTheScenario) { +TEST_CASE("Cli.ReadsWhatOverridesTheScenario") { const CliOptions options = parse( - {"--model", "m.xml", "--out", "o", "--command", "explore", "--command-at", "1.5", "--button", "extra_long", - "--button-at", "2.0", "--dip", "fan=1,risky=1", "--no-fan"} + {"--out", "o", "--scenario", "explore.toml", "--maze", "apec2018", "--seconds", "4", "--seed", "7", "--ideal", + "--record-every", "8"} ); - EXPECT_EQ(options.scenario.command, Command::EXPLORE); - EXPECT_DOUBLE_EQ(options.scenario.command_at, 1.5); - EXPECT_EQ(options.scenario.button, ButtonPress::EXTRA_LONG); - EXPECT_DOUBLE_EQ(options.scenario.button_at, 2.0); - EXPECT_EQ(options.scenario.dip, (std::array{true, false, false, true})); - EXPECT_FALSE(options.scenario.fan_enabled); + CHECK_EQ(options.scenario, "explore.toml"); + CHECK_EQ(options.maze, "apec2018"); + CHECK_EQ(options.seconds, 4.0); + CHECK_EQ(options.seed, 7U); + CHECK(options.ideal); + CHECK_EQ(options.record_every, 8U); + CHECK_EQ(parse({"--out", "o", "--ticks", "12"}).ticks, 12U); } -TEST(Cli, ReadsTheRecordingOptions) { +TEST_CASE("Cli.ReadsTheRecordingOptions") { const CliOptions options = parse( - {"--model", "m.xml", "--out", "o", "--video", "run.mp4", "--video-fps", "60", "--video-camera", "overhead", - "--video-size", "640x480"} + {"--out", "o", "--video", "run.mp4", "--video-fps", "60", "--video-camera", "overhead", "--video-size", + "640x480", "--video-trail"} ); - EXPECT_TRUE(options.video_enabled); - EXPECT_EQ(options.video.path, "run.mp4"); - EXPECT_EQ(options.video.fps, 60); - EXPECT_EQ(options.video.camera, "overhead"); - EXPECT_EQ(options.video.width, 640); - EXPECT_EQ(options.video.height, 480); + CHECK(options.video.trail); + CHECK_EQ(options.video.path, "run.mp4"); + CHECK_EQ(options.video.fps, 60); + CHECK_EQ(options.video.camera, "overhead"); + CHECK_EQ(options.video.width, 640); + CHECK_EQ(options.video.height, 480); } -TEST(Cli, ReadsTheWindowOptions) { - const CliOptions off = parse({"--model", "m.xml", "--out", "o"}); - EXPECT_FALSE(off.viewer_enabled); - +TEST_CASE("Cli.ReadsTheWindowOptions") { const CliOptions options = parse( - {"--model", "m.xml", "--out", "o", "--viewer", "--viewer-camera", "overhead", "--viewer-size", "800x600", - "--viewer-fps", "15"} + {"--out", "o", "--viewer", "--viewer-camera", "overhead", "--viewer-size", "800x600", "--viewer-fps", "15"} ); - EXPECT_TRUE(options.viewer_enabled); - EXPECT_EQ(options.viewer.camera, "overhead"); - EXPECT_EQ(options.viewer.width, 800); - EXPECT_EQ(options.viewer.height, 600); - EXPECT_EQ(options.viewer_fps, 15); + CHECK(options.viewer_enabled); + CHECK_EQ(options.viewer.camera, "overhead"); + CHECK_EQ(options.viewer.width, 800); + CHECK_EQ(options.viewer.height, 600); + CHECK_EQ(options.viewer_fps, 15); } -TEST(Cli, RejectsAnIncompleteCommandLine) { - EXPECT_THROW(parse({"--out", "o"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--nonsense"}), std::runtime_error); +TEST_CASE("Cli.RejectsAnIncompleteCommandLine") { + CHECK_THROWS_AS(parse({"--seconds", "1"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--nonsense"}), std::runtime_error); } -TEST(Cli, RejectsValuesTheRunCouldNotHonour) { - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--seconds", "0"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--ticks", "0"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--seconds", "0.0005"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--command", "wander"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--button", "double"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--dip", "turbo=1"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--dip", "fan=2"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--video", "v.mp4", "--video-fps", "0"}), std::runtime_error); - EXPECT_THROW( - parse({"--model", "m.xml", "--out", "o", "--video", "v.mp4", "--video-size", "big"}), std::runtime_error - ); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--viewer", "--viewer-size", "0x0"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--viewer", "--viewer-fps", "0"}), std::runtime_error); - EXPECT_THROW(parse({"--model", "m.xml", "--out", "o", "--viewer", "--viewer-size", "wide"}), std::runtime_error); +TEST_CASE("Cli.RejectsValuesTheRunCouldNotHonor") { + CHECK_THROWS_AS(parse({"--out", "o", "--seconds", "0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--ticks", "0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--record-every", "0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--video", "v.mp4", "--video-fps", "0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--video", "v.mp4", "--video-size", "big"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--viewer", "--viewer-size", "0x0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--viewer", "--viewer-fps", "0"}), std::runtime_error); + CHECK_THROWS_AS(parse({"--out", "o", "--viewer", "--viewer-size", "wide"}), std::runtime_error); +} + +TEST_CASE("Cli.HandsTargetOptionsToTheirHandlers") { + std::string color; + bool flag = false; + const std::vector target_options{ + {.name = "--color", .argument = "", .apply = [&color](const std::string& value) { color = value; }}, + {.name = "--flag", .argument = "", .apply = [&flag](const std::string&) { flag = true; }}, + }; + + const CliOptions options = parse({"--color", "red", "--maze", "m", "--flag", "--out", "o"}, target_options); + + CHECK_EQ(color, "red"); + CHECK(flag); + CHECK_EQ(options.maze, "m"); } -TEST(Cli, UsageNamesEveryOptionItAccepts) { - const std::string usage = Cli::usage(); +TEST_CASE("Cli.UsageNamesEveryOptionItAccepts") { + const std::vector target_options{{.name = "--color", .argument = "", .apply = {}}}; + const std::string usage = Cli::usage("robot_sim", target_options); + + CHECK(usage.starts_with("usage: robot_sim ")); - for (const std::string& option : - {"--model", "--out", "--maze", "--seconds", "--ticks", "--command", "--command-at", "--dip", "--button", - "--button-at", "--no-fan", "--video", "--video-fps", "--video-camera", "--video-size", "--viewer", - "--viewer-camera", "--viewer-fps", "--viewer-size"}) { - EXPECT_NE(usage.find(option), std::string::npos) << option; + for (const std::string option : + {"--out", "--scenario", "--maze", "--seconds", "--ticks", + "--seed", "--ideal", "--record-every", "--video", "--video-fps", + "--video-camera", "--video-size", "--video-trail", "--viewer", "--viewer-camera", + "--viewer-fps", "--viewer-size", "--monitor", "--monitor-port", "--color "}) { + CHECK_MESSAGE(usage.find(option) != std::string::npos, option); } } } // namespace diff --git a/tests/unit/clock_test.cpp b/tests/unit/clock_test.cpp index 16302c7..7929ce4 100644 --- a/tests/unit/clock_test.cpp +++ b/tests/unit/clock_test.cpp @@ -1,62 +1,50 @@ -#include +#include + +#include #include "micras/sim/core/clock.hpp" namespace micras::sim { -namespace { /** - * @brief The pair every recorded run uses: a 521 us timestep and a 1042 us loop. + * @brief A timestep that divides the loop into two steps. */ -Clock harness_clock() { +static Clock harness_clock() { return Clock::from_model(0.000521, 1042); } -TEST(Clock, DerivesTheStepCountFromTheModel) { +namespace { +TEST_CASE("Clock.DerivesTheStepCountFromTheModel") { const Clock clock = harness_clock(); - EXPECT_EQ(clock.steps_per_tick(), 2); - EXPECT_EQ(clock.us_per_tick(), 1042U); - EXPECT_EQ(clock.now_us(), 0U); - EXPECT_EQ(clock.tick_count(), 0U); + CHECK_EQ(clock.steps_per_tick(), 2); + CHECK_EQ(clock.us_per_tick(), 1042U); + CHECK_EQ(clock.now_us(), 0U); + CHECK_EQ(clock.tick_count(), 0U); } -TEST(Clock, RejectsATimestepThatDoesNotDivideTheLoop) { - EXPECT_THROW(Clock::from_model(0.0004, 1042), std::runtime_error); - EXPECT_THROW(Clock::from_model(0.002, 1042), std::runtime_error); +TEST_CASE("Clock.RejectsATimestepThatDoesNotDivideTheLoop") { + CHECK_THROWS_AS(Clock::from_model(0.0004, 1042), std::runtime_error); + CHECK_THROWS_AS(Clock::from_model(0.002, 1042), std::runtime_error); } -TEST(Clock, AdvancesOneTickAtATime) { +TEST_CASE("Clock.AdvancesOneTickAtATime") { Clock clock = harness_clock(); clock.advance(); clock.advance(); - EXPECT_EQ(clock.tick_count(), 2U); - EXPECT_EQ(clock.now_us(), 2084U); - - clock.reset(); - EXPECT_EQ(clock.tick_count(), 0U); - EXPECT_EQ(clock.now_us(), 0U); + CHECK_EQ(clock.tick_count(), 2U); + CHECK_EQ(clock.now_us(), 2084U); } -TEST(Clock, CountsTheTicksOfTheRecordedScenarios) { +TEST_CASE("Clock.CountsTheWholeTicksOfADuration") { const Clock clock = harness_clock(); - EXPECT_EQ(clock.total_ticks(4.0), 3838U); - EXPECT_EQ(clock.total_ticks(8.0), 7677U); + CHECK_EQ(clock.total_ticks(4.0), 3838U); + CHECK_EQ(clock.total_ticks(8.0), 7677U); } -TEST(Clock, RoundsInstantsToTheNearestTick) { +TEST_CASE("Clock.RoundsInstantsToTheNearestTick") { const Clock clock = harness_clock(); - EXPECT_EQ(clock.tick_at(0.5), 480U); - EXPECT_EQ(clock.tick_at(0.0), 0U); -} - -TEST(Clock, CountsTheTicksTheButtonMustBeHeldFor) { - const Clock clock = harness_clock(); - - for (const uint32_t target : {250U, 501U, 2001U}) { - const uint64_t ticks = clock.ticks_for_elapsed_ms(target); - EXPECT_GE(ticks * clock.us_per_tick() / 1000, target); - EXPECT_LT((ticks - 1) * clock.us_per_tick() / 1000, target); - } + CHECK_EQ(clock.tick_at(0.5), 480U); + CHECK_EQ(clock.tick_at(0.0), 0U); } } // namespace } // namespace micras::sim diff --git a/tests/unit/csv_recorder_test.cpp b/tests/unit/csv_recorder_test.cpp index 2cba800..69f94ed 100644 --- a/tests/unit/csv_recorder_test.cpp +++ b/tests/unit/csv_recorder_test.cpp @@ -1,96 +1,188 @@ -#include +#include +#include +#include +#include +#include +#include -#include +#include +#include "micras/sim/recording/column_source.hpp" #include "micras/sim/recording/csv_recorder.hpp" +#include "micras/sim/recording/csv_writer.hpp" #include "micras/sim/recording/ground_truth.hpp" +#include "temp_file.hpp" namespace micras::sim { +/** + * @brief Ground truth of the tiny robot: one column of each kind the probes cover. + */ +static GroundTruthConfig tiny_config() { + return { + .body = "robot", + .columns = { + {.name = "wheel_speed", .probe = Probe::JOINT_VELOCITY, .object = "wheel"}, + {.name = "wheel_angle", .probe = Probe::JOINT_POSITION, .object = "wheel"}, + {.name = "torque", .probe = Probe::ACTUATOR_FORCE, .object = "motor"}, + {.name = "chassis_ncon", .probe = Probe::CONTACT_COUNT, .object = "chassis"}, + {.name = "chassis_fn", .probe = Probe::CONTACT_NORMAL_FORCE, .object = "chassis"}, + {.name = "chassis_slip", .probe = Probe::CONTACT_SLIP, .object = "chassis"}, + {.name = "chassis_penetration", .probe = Probe::CONTACT_PENETRATION, .object = "chassis"}, + {.name = "iterations", .probe = Probe::SOLVER_ITERATIONS, .object = ""}, + }, + }; +} + namespace { /** - * @brief Ground truth header of every run recorded so far, in order. - * - * @note Transcribed from baseline/v0/idle/data.csv. Changing it breaks the - * comparison against every run already on disk, so it is pinned here - * instead of being derived from the code under test. + * @brief A column source with fixed names and values. */ -const std::vector baseline_columns{ - "tick", - "sim_time", - "fw_loop_time", - "x", - "y", - "z", - "roll", - "pitch", - "yaw", - "vx_world", - "vy_world", - "vz_world", - "wz_body", - "v_forward", - "wheel_qvel_left", - "wheel_qvel_right", - "ctrl_left", - "ctrl_right", - "act_force_left", - "act_force_right", - "left_ncon", - "left_fn", - "left_slip", - "left_penetration", - "right_ncon", - "right_fn", - "right_slip", - "right_penetration", - "caster_ncon", - "ncon_total", - "solver_niter", - "warnings_total", - "wheel_qpos_left", - "wheel_qpos_right", - "caster_fn", - "base_ncon" +class FixedColumns : public ColumnSource { +public: + explicit FixedColumns(std::size_t cells_per_row) : cells_per_row{cells_per_row} { } + + std::vector names() override { return {"first", "second"}; } + + void append(std::vector& row) override { + for (std::size_t i = 0; i < this->cells_per_row; i++) { + row.emplace_back(static_cast(i)); + } + } + +private: + std::size_t cells_per_row; }; -TEST(GroundTruth, ColumnsMatchTheRecordedBaseline) { - EXPECT_EQ(GroundTruth::columns(), baseline_columns); +/** + * @brief A column source that can name its columns only once it is told it can. + */ +class LateColumns : public FixedColumns { +public: + LateColumns() : FixedColumns{2} { } + + std::vector names() override { return {"third", "fourth"}; } + + bool ready() const override { return this->started; } + + void start() { this->started = true; } + +private: + bool started{false}; +}; + +class Recording { +protected: + Recording() { + this->world.load(MICRAS_TEST_MODEL); + this->world.reset(); + } + + /** + * @brief Read back every line of the CSV. + */ + std::vector lines() const { + std::ifstream file(this->csv.path()); + std::vector read; + + for (std::string line; std::getline(file, line);) { + read.push_back(line); + } + + return read; + } + + /** + * @brief Read back the first line of the CSV. + */ + std::string header() const { + std::ifstream file(this->csv.path()); + std::string line; + std::getline(file, line); + return line; + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + MujocoWorld world; + TempFile csv{"recorder_test.csv"}; + // NOLINTEND(*-non-private-member-variables-in-classes) +}; + +TEST_CASE_FIXTURE(Recording, "Recording.StartsEveryRowWithTheBodyBlockThenTheRobotColumns") { + const GroundTruth ground_truth(this->world, tiny_config()); + const std::vector columns = ground_truth.columns(); + + REQUIRE_EQ(columns.size(), 13U + tiny_config().columns.size()); + CHECK_EQ(columns.front(), "tick"); + CHECK_EQ(columns.at(12), "v_forward"); + CHECK_EQ(columns.at(13), "wheel_speed"); + CHECK_EQ(columns.back(), "iterations"); } -TEST(CsvRecorder, ProxyColumnsMatchTheProxyStateSections) { - const std::vector expected{ - "proxy_left_command", "proxy_right_command", "proxy_fan_speed", "proxy_encoder_left", "proxy_encoder_right", - "proxy_gyro_x", "proxy_gyro_y", "proxy_gyro_z", "proxy_accel_x", "proxy_accel_y", - "proxy_accel_z", "proxy_wall_adc_0", "proxy_wall_adc_1", "proxy_wall_adc_2", "proxy_wall_adc_3", - "proxy_button_pressed", "proxy_dip_0", "proxy_dip_1", "proxy_dip_2", "proxy_dip_3" - }; +TEST_CASE_FIXTURE(Recording, "Recording.WritesTheTickAndOneCellPerColumn") { + GroundTruth ground_truth(this->world, tiny_config()); + const std::vector row = ground_truth.sample(7); + + REQUIRE_EQ(row.size(), ground_truth.columns().size()); + CHECK_EQ(std::get(row.at(0)), 7U); + CHECK_EQ(std::get(row.at(1)), doctest::Approx(this->world.data()->time).epsilon(1e-12)); +} - EXPECT_EQ(CsvRecorder::proxy_columns(), expected); +TEST_CASE_FIXTURE(Recording, "Recording.RejectsAnObjectTheModelDoesNotHave") { + GroundTruthConfig config = tiny_config(); + config.columns.push_back({.name = "ghost", .probe = Probe::JOINT_VELOCITY, .object = "ghost"}); - const std::size_t actuators = 3; - const std::size_t sensors = Sensors::wheel_count + (2 * Sensors::axis_count) + Sensors::wall_sensor_count; - const std::size_t input = 1 + InterfaceInput::dip_switch_count; - EXPECT_EQ(CsvRecorder::proxy_columns().size(), actuators + sensors + input); + CHECK_THROWS_AS(GroundTruth(this->world, config), std::runtime_error); } -TEST(CsvRecorder, ProxyColumnsComeAfterEveryBaselineColumn) { - for (const std::string& column : CsvRecorder::proxy_columns()) { - EXPECT_TRUE(column.starts_with("proxy_")) << column; - EXPECT_EQ(std::count(baseline_columns.begin(), baseline_columns.end(), column), 0) << column; +TEST_CASE_FIXTURE(Recording, "Recording.AppendsTheSourcesAfterTheGroundTruth") { + FixedColumns source(2); + + { + CsvRecorder recorder(this->world, tiny_config(), this->csv.path()); + recorder.add_source(source); + recorder.sample(0); + } + + CHECK(this->header().ends_with(",iterations,first,second")); +} + +TEST_CASE_FIXTURE(Recording, "Recording.WritesNoRowBeforeEverySourceIsReady") { + FixedColumns early(2); + LateColumns late; + + { + CsvRecorder recorder(this->world, tiny_config(), this->csv.path()); + recorder.add_source(early); + recorder.add_source(late); + recorder.sample(1); + recorder.sample(2); + late.start(); + recorder.sample(3); } + + const std::vector written = this->lines(); + + REQUIRE_EQ(written.size(), 2U); + CHECK(written.at(0).starts_with("tick,")); + CHECK(written.at(1).starts_with("3,")); } -TEST(CsvRecorder, TurnsPoolNamesIntoTheBaselineColumnNames) { - EXPECT_EQ(CsvRecorder::column_name("Grid Pose"), "grid_pose"); - EXPECT_EQ(CsvRecorder::column_name("Desired Linear Speed"), "desired_linear_speed"); - EXPECT_EQ(CsvRecorder::column_name("Wall Sensors 0"), "wall_sensors_0"); - EXPECT_EQ(CsvRecorder::column_name("Linear/Angular PID"), "linear_angular_pid"); +TEST_CASE_FIXTURE(Recording, "Recording.RejectsASourceThatFillsTooFewCells") { + FixedColumns source(1); + CsvRecorder recorder(this->world, tiny_config(), this->csv.path()); + recorder.add_source(source); + + CHECK_THROWS_AS(recorder.sample(0), std::logic_error); } -TEST(CsvRecorder, TrimsSeparatorsAtTheEdgesOfPoolNames) { - EXPECT_EQ(CsvRecorder::column_name(" Spaced "), "spaced"); - EXPECT_EQ(CsvRecorder::column_name("Trailing!"), "trailing"); - EXPECT_EQ(CsvRecorder::column_name("!!!"), ""); +TEST_CASE_FIXTURE(Recording, "Recording.RejectsTwoSourcesWritingTheSameColumn") { + FixedColumns first(2); + FixedColumns second(2); + CsvRecorder recorder(this->world, tiny_config(), this->csv.path()); + recorder.add_source(first); + recorder.add_source(second); + + CHECK_THROWS_AS(recorder.sample(0), std::runtime_error); } } // namespace } // namespace micras::sim diff --git a/tests/unit/csv_writer_test.cpp b/tests/unit/csv_writer_test.cpp index 448f3ed..e1216e5 100644 --- a/tests/unit/csv_writer_test.cpp +++ b/tests/unit/csv_writer_test.cpp @@ -1,59 +1,54 @@ #include -#include -#include +#include #include -#include +#include +#include +#include -#include +#include +#include "micras/sim/core/text_file.hpp" #include "micras/sim/recording/csv_writer.hpp" +#include "temp_file.hpp" namespace micras::sim { namespace { -std::string read_file(const std::filesystem::path& path) { - const std::ifstream file(path); - std::stringstream buffer; - buffer << file.rdbuf(); - return buffer.str(); +TEST_CASE("CsvWriter.FormatsCellsWithTwelveSignificantDigits") { + CHECK_EQ(CsvWriter::format(CsvCell{0.001042}), "0.001042"); + CHECK_EQ(CsvWriter::format(CsvCell{3.99920000001}), "3.9992"); + CHECK_EQ(CsvWriter::format(CsvCell{0.00080690123456}), "0.000806901235"); + CHECK_EQ(CsvWriter::format(CsvCell{int64_t{-7}}), "-7"); + CHECK_EQ(CsvWriter::format(CsvCell{uint64_t{7677}}), "7677"); } -TEST(CsvWriter, FormatsCellsLikeTheHarnessAlwaysDid) { - EXPECT_EQ(CsvWriter::format(CsvCell{0.001042}), "0.001042"); - EXPECT_EQ(CsvWriter::format(CsvCell{3.99920000001}), "3.9992"); - EXPECT_EQ(CsvWriter::format(CsvCell{0.00080690123456}), "0.000806901235"); - EXPECT_EQ(CsvWriter::format(CsvCell{int64_t{-7}}), "-7"); - EXPECT_EQ(CsvWriter::format(CsvCell{uint64_t{7677}}), "7677"); +TEST_CASE("CsvWriter.FormatsTheSpecialValuesTheBaselineDependsOn") { + CHECK_EQ(CsvWriter::format(CsvCell{std::nan("")}), "nan"); + CHECK_EQ(CsvWriter::format(CsvCell{-std::nan("")}), "-nan"); + CHECK_EQ(CsvWriter::format(CsvCell{std::numeric_limits::infinity()}), "inf"); + CHECK_EQ(CsvWriter::format(CsvCell{-std::numeric_limits::infinity()}), "-inf"); + CHECK_EQ(CsvWriter::format(CsvCell{-0.0}), "-0"); + CHECK_EQ(CsvWriter::format(CsvCell{0.0}), "0"); + CHECK_EQ(CsvWriter::format(CsvCell{1e20}), "1e+20"); + CHECK_EQ(CsvWriter::format(CsvCell{1e-5}), "1e-05"); + CHECK_EQ(CsvWriter::format(CsvCell{9.99999999e-5}), "9.99999999e-05"); } -TEST(CsvWriter, FormatsTheSpecialValuesTheBaselineDependsOn) { - EXPECT_EQ(CsvWriter::format(CsvCell{std::nan("")}), "nan"); - EXPECT_EQ(CsvWriter::format(CsvCell{-std::nan("")}), "-nan"); - EXPECT_EQ(CsvWriter::format(CsvCell{std::numeric_limits::infinity()}), "inf"); - EXPECT_EQ(CsvWriter::format(CsvCell{-std::numeric_limits::infinity()}), "-inf"); - EXPECT_EQ(CsvWriter::format(CsvCell{-0.0}), "-0"); - EXPECT_EQ(CsvWriter::format(CsvCell{0.0}), "0"); - EXPECT_EQ(CsvWriter::format(CsvCell{1e20}), "1e+20"); - EXPECT_EQ(CsvWriter::format(CsvCell{1e-5}), "1e-05"); - EXPECT_EQ(CsvWriter::format(CsvCell{9.99999999e-5}), "9.99999999e-05"); -} - -TEST(CsvWriter, WritesHeaderAndRows) { - const std::filesystem::path path = std::filesystem::temp_directory_path() / "micras_csv_writer_test.csv"; +TEST_CASE("CsvWriter.WritesHeaderAndRows") { + const TempFile csv{"csv_writer_test.csv"}; { - CsvWriter writer(path); + CsvWriter writer(csv.path()); const std::vector columns{"tick", "x", "count"}; writer.write_header(columns); writer.write_row(std::vector{uint64_t{0}, 0.5, int64_t{2}}); writer.write_row(std::vector{uint64_t{1}, std::nan(""), int64_t{0}}); } - EXPECT_EQ(read_file(path), "tick,x,count\n0,0.5,2\n1,nan,0\n"); - std::filesystem::remove(path); + CHECK_EQ(read_text_file(csv.path(), "CSV"), "tick,x,count\n0,0.5,2\n1,nan,0\n"); } -TEST(CsvWriter, ThrowsWhenTheFileCannotBeOpened) { - EXPECT_THROW(CsvWriter("/nonexistent/directory/file.csv"), std::runtime_error); +TEST_CASE("CsvWriter.ThrowsWhenTheFileCannotBeOpened") { + CHECK_THROWS_AS(CsvWriter("/nonexistent/directory/file.csv"), std::runtime_error); } } // namespace } // namespace micras::sim diff --git a/tests/unit/devices_test.cpp b/tests/unit/devices_test.cpp new file mode 100644 index 0000000..1a5c780 --- /dev/null +++ b/tests/unit/devices_test.cpp @@ -0,0 +1,372 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/devices/dc_motor.hpp" +#include "micras/sim/devices/digital_input.hpp" +#include "micras/sim/devices/quadrature_encoder.hpp" +#include "micras/sim/devices/serial_link.hpp" +#include "micras/sim/recording/csv_writer.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "support.hpp" + +namespace micras::sim { +/** + * @brief A made-up drive: a 6 V supply, a 10 ohm path, a 5:1 gearbox. + */ +static DriveDescription tiny_drive() { + return { + .winding_resistance = 9.5, + .bridge_resistance = 0.5, + .torque_constant = 0.003, + .speed_constant = 0.003, + .no_load_current = 0.007, + .rotor_inertia = 4.0e-9, + .gear_ratio = 5.0, + .gear_efficiency = 0.9, + .supply_voltage = 6.0, + }; +} + +namespace { +/** + * @brief The tiny robot's world and a clock of 1042 us ticks. + */ +class Devices { +protected: + Devices() { load_tiny_world(this->world, this->clock); } + + /** + * @brief Get the tiny robot's wheel joint position. + * + * @return The joint's entry of qpos. + */ + mjtNum& wheel_angle() { + const std::span positions(this->world.data()->qpos, static_cast(this->world.model()->nq)); + const std::span addresses( + this->world.model()->jnt_qposadr, static_cast(this->world.model()->njnt) + ); + return at(positions, static_cast(at(addresses, this->wheel()))); + } + + /** + * @brief Get the tiny robot's wheel joint velocity. + * + * @return The joint's entry of qvel. + */ + mjtNum& wheel_speed() { + const std::span velocities(this->world.data()->qvel, static_cast(this->world.model()->nv)); + const std::span addresses( + this->world.model()->jnt_dofadr, static_cast(this->world.model()->njnt) + ); + return at(velocities, static_cast(at(addresses, this->wheel()))); + } + + /** + * @brief Get the control the motor actuator was given. + * + * @return The actuator's entry of ctrl. + */ + double motor_control() const { + const std::span controls( + this->world.data()->ctrl, static_cast(this->world.model()->nu) + ); + return at(controls, static_cast(this->world.require_id(mjOBJ_ACTUATOR, "motor"))); + } + + /** + * @brief Build a motor on the tiny robot's wheel, driven by the fixture's bridge inputs. + * + * @return The motor. + */ + DcMotor motor() { + return { + this->world, + {.name = "wheel", + .actuator = "motor", + .joint = "wheel", + .drive = tiny_drive(), + .forward_duty = [this] { return this->forward; }, + .backward_duty = [this] { return this->backward; }, + .enabled = [this] { return this->enabled; }} + }; + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + MujocoWorld world; + Clock clock; + float forward{0.0F}; + float backward{0.0F}; + bool enabled{true}; + // NOLINTEND(*-non-private-member-variables-in-classes) + +private: + /** + * @brief Get the id of the tiny robot's wheel joint. + * + * @return The id. + */ + std::size_t wheel() const { return static_cast(this->world.require_id(mjOBJ_JOINT, "wheel")); } +}; + +TEST_CASE_FIXTURE(Devices, "Devices.AppliesTheDutyCycleDifferenceTimesTheSupply") { + DcMotor motor = this->motor(); + + this->forward = 50.0F; + motor.actuate(this->world, this->clock); + CHECK_EQ(this->motor_control(), doctest::Approx(3.0).epsilon(1e-12)); + + this->forward = 25.0F; + this->backward = 75.0F; + motor.actuate(this->world, this->clock); + CHECK_EQ(this->motor_control(), doctest::Approx(-3.0).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Devices, "Devices.NeverAppliesMoreThanTheSupply") { + DcMotor motor = this->motor(); + + this->forward = 150.0F; + motor.actuate(this->world, this->clock); + + CHECK_EQ(this->motor_control(), doctest::Approx(6.0).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Devices, "Devices.DrivesTheCurrentAgainstTheBackEmf") { + DcMotor motor = this->motor(); + this->wheel_speed() = 10.0; + + this->forward = 50.0F; + motor.actuate(this->world, this->clock); + + const double back_emf = 0.003 * 5.0 * 10.0; + CHECK_EQ(motor.current(), doctest::Approx((3.0 - back_emf) / 10.0).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Devices, "Devices.BrakesWithBothInputsLow") { + DcMotor motor = this->motor(); + this->wheel_speed() = 10.0; + + motor.actuate(this->world, this->clock); + + CHECK_EQ(this->motor_control(), doctest::Approx(0.0).epsilon(1e-12)); + CHECK_LT(motor.current(), 0.0); +} + +TEST_CASE_FIXTURE(Devices, "Devices.LeavesTheWindingOpenWhenTheBridgeIsDisabled") { + DcMotor motor = this->motor(); + this->wheel_speed() = 10.0; + this->forward = 100.0F; + this->enabled = false; + + motor.actuate(this->world, this->clock); + + CHECK_EQ(this->motor_control(), doctest::Approx(0.003 * 5.0 * 10.0).epsilon(1e-12)); + CHECK_EQ(motor.current(), doctest::Approx(0.0).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Devices, "Devices.RecordsTheMotorsVoltageAndCurrent") { + DcMotor motor = this->motor(); + this->forward = 50.0F; + motor.actuate(this->world, this->clock); + + std::vector row; + motor.append(row); + + CHECK_EQ(motor.columns(), (std::vector{"wheel_voltage", "wheel_current"})); + REQUIRE_EQ(row.size(), 2U); + CHECK_EQ(std::get(row.at(0)), doctest::Approx(3.0).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Devices, "Devices.RefusesAMotorOnAnActuatorTheModelDoesNotHave") { + CHECK_THROWS_AS( + DcMotor( + this->world, {.name = "ghost", + .actuator = "ghost", + .joint = "wheel", + .drive = tiny_drive(), + .forward_duty = [] { return 0.0F; }, + .backward_duty = [] { return 0.0F; }, + .enabled = [] { return true; }} + ), + std::runtime_error + ); +} + +TEST_CASE_FIXTURE(Devices, "Devices.CountsTheWheelAngleAtTheEncodersResolution") { + std::optional written; + QuadratureEncoder encoder( + this->world, + {.name = "wheel", .joint = "wheel", .counts_per_revolution = 4096, .write = [&written](int32_t count) { + written = count; + }} + ); + const double radians_per_count = 2.0 * std::numbers::pi / 4096.0; + + this->wheel_angle() = 1024.5 * radians_per_count; + encoder.sample(this->world, this->clock); + CHECK_EQ(written, 1024); + + this->wheel_angle() = (2.5 * 4096.0 + 0.5) * radians_per_count; + encoder.sample(this->world, this->clock); + CHECK_EQ(written, 10240); +} + +TEST_CASE_FIXTURE(Devices, "Devices.CountsDownWhenTheWheelTurnsBackwards") { + std::optional written; + QuadratureEncoder encoder( + this->world, + {.name = "wheel", .joint = "wheel", .counts_per_revolution = 4096, .write = [&written](int32_t count) { + written = count; + }} + ); + + this->wheel_angle() = -1023.5 * 2.0 * std::numbers::pi / 4096.0; + encoder.sample(this->world, this->clock); + + CHECK_EQ(written, -1024); + + std::vector row; + encoder.append(row); + CHECK_EQ(encoder.columns(), (std::vector{"wheel_count"})); + CHECK_EQ(std::get(row.at(0)), -1024); +} + +/** + * @brief A link at 115200 baud, fed from a queue the firmware fills. + */ +class Link : public Devices { +protected: + /** + * @brief Build the link. + * + * @return The link, on the fixture's bus. + */ + SerialLink link() { + return { + this->bus, + {.baud_rate = 115200, + .take_sent = [this]() -> std::optional { + if (this->outgoing.empty()) { + return std::nullopt; + } + + const uint8_t byte = this->outgoing.front(); + this->outgoing.pop_front(); + return byte; + }, + .receive = [this](uint8_t byte) { this->incoming.push_back(byte); }} + }; + } + + /** + * @brief Queue bytes for the firmware to send. + * + * @param count Number of bytes, counting up from 0. + */ + void firmware_sends(std::size_t count) { + for (std::size_t i = 0; i < count; i++) { + this->outgoing.push_back(static_cast(i)); + } + } + + /** + * @brief Bytes the line carries in one tick. + * + * @return 115200 baud, ten bits a byte, over 1042 us. + */ + static double bytes_per_tick() { return 115200.0 / 10.0 * 1042e-6; } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + SerialBus bus; + ByteCollector collector; + std::deque outgoing; + std::vector incoming; + // NOLINTEND(*-non-private-member-variables-in-classes) +}; + +TEST_CASE_FIXTURE(Link, "Link.SendsNoMoreThanTheBaudRateAllows") { + SerialLink link = this->link(); + this->bus.add_listener(this->collector); + this->firmware_sends(5000); + + link.sample(this->world, this->clock); + CHECK_EQ(this->collector.received.size(), static_cast(bytes_per_tick())); + + for (int tick = 1; tick < 100; tick++) { + link.sample(this->world, this->clock); + } + + CHECK_EQ(this->collector.received.size(), static_cast(100 * bytes_per_tick())); + CHECK_EQ(this->collector.received.at(200), 200U); +} + +TEST_CASE_FIXTURE(Link, "Link.ReceivesNoMoreThanTheBaudRateAllows") { + SerialLink link = this->link(); + const std::vector message(5000, 0x55); + this->bus.queue_for_firmware(message); + + link.sample(this->world, this->clock); + CHECK_EQ(this->incoming.size(), static_cast(bytes_per_tick())); + + for (int tick = 1; tick < 100; tick++) { + link.sample(this->world, this->clock); + } + + CHECK_EQ(this->incoming.size(), static_cast(100 * bytes_per_tick())); +} + +TEST_CASE_FIXTURE(Link, "Link.DoesNotSaveUpBandwidthWhileIdle") { + SerialLink link = this->link(); + this->bus.add_listener(this->collector); + + for (int tick = 0; tick < 100; tick++) { + link.sample(this->world, this->clock); + } + + this->firmware_sends(5000); + this->bus.queue_for_firmware(std::vector(5000, 0x55)); + link.sample(this->world, this->clock); + + CHECK_LE(this->collector.received.size(), static_cast(1.0 + bytes_per_tick())); + CHECK_LE(this->incoming.size(), static_cast(1.0 + bytes_per_tick())); +} + +TEST_CASE_FIXTURE(Link, "Link.CarriesASlowTrickleWholeAndInOrder") { + SerialLink link = this->link(); + this->bus.add_listener(this->collector); + + for (int tick = 0; tick < 10; tick++) { + this->outgoing.push_back(static_cast(tick)); + link.sample(this->world, this->clock); + } + + CHECK_EQ(this->collector.received, (std::vector{0, 1, 2, 3, 4, 5, 6, 7, 8, 9})); +} + +TEST_CASE_FIXTURE(Devices, "Devices.DrivesADigitalInputsPinWithItsPolarity") { + std::optional pin; + DigitalInput input({.name = "switch", .active_low = true, .drive = [&pin](bool level) { pin = level; }}); + + CHECK_FALSE(input.is_active()); + CHECK_EQ(pin, true); + + input.set(true); + CHECK(input.is_active()); + CHECK_EQ(pin, false); + CHECK_EQ(input.name(), "switch"); +} +} // namespace +} // namespace micras::sim diff --git a/tests/unit/event_log_test.cpp b/tests/unit/event_log_test.cpp new file mode 100644 index 0000000..fff84b9 --- /dev/null +++ b/tests/unit/event_log_test.cpp @@ -0,0 +1,190 @@ +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/recording/event_log.hpp" +#include "support.hpp" + +namespace micras::sim { +namespace { +/** + * @brief The tiny robot, lifted clear of the floor or lowered onto it at chosen instants. + */ +class Logging { +protected: + Logging() { + load_tiny_world(this->context.world, this->context.clock); + this->chassis = this->context.world.require_id(mjOBJ_GEOM, "chassis"); + this->floor = this->context.world.require_id(mjOBJ_GEOM, "floor"); + } + + /** + * @brief Put the robot on the floor or in the air at an instant, and let a log see it. + * + * @param log Log to show the tick to. + * @param time Simulated time of the tick. + * @param touching Whether the chassis rests on the floor. + */ + void tick(EventLog& log, double time, bool touching) { + MujocoWorld& world = this->context.world; + const std::span positions(world.data()->qpos, static_cast(world.model()->nq)); + + at(positions, 2) = touching ? 0.005 : 0.1; + world.data()->time = time; + mj_forward(world.model(), world.data()); + log.on_after_tick(this->simulation); + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + RunContext context; + FirmwareThread firmware{[](FirmwareThread&) { }}; + Simulation simulation{this->context, this->firmware}; + MapVariables variables; + int chassis{-1}; + int floor{-1}; + // NOLINTEND(*-non-private-member-variables-in-classes) +}; + +TEST_CASE_FIXTURE(Logging, "Logging.LogsOneCollisionPerContactEpisode") { + EventLog log({this->chassis}, {}, {}, nullptr); + + this->tick(log, 0.0, false); + this->tick(log, 0.001, true); + this->tick(log, 0.002, true); + this->tick(log, 0.003, true); + + CHECK_EQ(log.collisions(), 1U); + REQUIRE_EQ(log.entries().size(), 1U); + CHECK_EQ(log.entries().front().kind, "collision"); + CHECK_EQ(log.entries().front().detail, "chassis"); + CHECK_EQ(log.entries().front().time, doctest::Approx(0.001).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Logging, "Logging.CountsAContactWithinTheSeparationTimeAsTheSameCollision") { + EventLog log({this->chassis}, {}, {}, nullptr); + + this->tick(log, 0.0, true); + this->tick(log, 0.01, false); + this->tick(log, 0.03, false); + this->tick(log, 0.04, true); + + CHECK_EQ(log.collisions(), 1U); +} + +TEST_CASE_FIXTURE(Logging, "Logging.CountsAContactAfterTheSeparationTimeAsANewCollision") { + EventLog log({this->chassis}, {}, {}, nullptr); + + this->tick(log, 0.0, true); + this->tick(log, 0.02, true); + this->tick(log, 0.05, false); + this->tick(log, 0.08, true); + + CHECK_EQ(log.collisions(), 2U); + REQUIRE_EQ(log.entries().size(), 2U); + CHECK_EQ(log.entries().back().time, doctest::Approx(0.08).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Logging, "Logging.NeverCountsAContactWithAnIgnoredGeom") { + EventLog log({this->chassis}, {this->floor}, {}, nullptr); + + this->tick(log, 0.0, true); + this->tick(log, 1.0, false); + this->tick(log, 2.0, true); + + CHECK_EQ(log.collisions(), 0U); + CHECK(log.entries().empty()); +} + +TEST_CASE_FIXTURE(Logging, "Logging.WatchesOnlyTheGeomsItWasGiven") { + EventLog log({}, {}, {}, nullptr); + + this->tick(log, 0.0, true); + + CHECK_EQ(log.collisions(), 0U); +} + +TEST_CASE_FIXTURE(Logging, "Logging.NamesEachNewStateFromItsTable") { + EventLog log({}, {}, {{"state", {"INIT", "IDLE", "RUN"}}}, &this->variables); + + this->tick(log, 0.0, false); + this->variables.set("state", 0); + this->tick(log, 0.1, false); + this->tick(log, 0.2, false); + this->variables.set("state", 1); + this->tick(log, 0.3, false); + this->variables.set("state", 2); + this->tick(log, 0.4, false); + + REQUIRE_EQ(log.entries().size(), 3U); + CHECK_EQ(log.entries().at(0).kind, "state"); + CHECK_EQ(log.entries().at(0).detail, "INIT"); + CHECK_EQ(log.entries().at(0).time, doctest::Approx(0.1).epsilon(1e-12)); + CHECK_EQ(log.entries().at(1).detail, "IDLE"); + CHECK_EQ(log.entries().at(2).detail, "RUN"); + CHECK_EQ(log.entries().at(2).time, doctest::Approx(0.4).epsilon(1e-12)); +} + +TEST_CASE_FIXTURE(Logging, "Logging.WritesAValueWithNoNameAsANumber") { + EventLog log({}, {}, {{"state", {"INIT"}}}, &this->variables); + + this->variables.set("state", 7); + this->tick(log, 0.0, false); + this->variables.set("state", -1); + this->tick(log, 0.1, false); + + REQUIRE_EQ(log.entries().size(), 2U); + CHECK_EQ(log.entries().at(0).detail, "7"); + CHECK_EQ(log.entries().at(1).detail, "-1"); +} + +TEST_CASE_FIXTURE(Logging, "Logging.SkipsAVariableThatIsNotReportedYet") { + EventLog log({}, {}, {{"state", {"INIT", "IDLE"}}}, &this->variables); + + this->variables.set("state", 1); + this->tick(log, 0.0, false); + this->variables.set("state", std::nan("")); + this->tick(log, 0.1, false); + this->variables.set("state", 1); + this->tick(log, 0.2, false); + + REQUIRE_EQ(log.entries().size(), 1U); + CHECK_EQ(log.entries().front().detail, "IDLE"); +} + +TEST_CASE_FIXTURE(Logging, "Logging.WatchesEveryVariableItWasGiven") { + EventLog log({}, {}, {{"mode", {"SLOW", "FAST"}}, {"state", {"INIT"}}}, &this->variables); + + this->variables.set("mode", 1); + this->tick(log, 0.0, false); + + REQUIRE_EQ(log.entries().size(), 1U); + CHECK_EQ(log.entries().front().kind, "mode"); + CHECK_EQ(log.entries().front().detail, "FAST"); +} + +TEST_CASE_FIXTURE(Logging, "Logging.KeepsTheFirstEntriesOnlyButKeepsCounting") { + EventLog log({this->chassis}, {}, {{"counter", {}}}, &this->variables); + + for (std::size_t i = 0; i < EventLog::max_events + 10; i++) { + this->variables.set("counter", static_cast(i)); + this->tick(log, static_cast(i), false); + } + + this->tick(log, 1000.0, true); + + CHECK_EQ(log.entries().size(), EventLog::max_events); + CHECK_EQ(log.entries().back().detail, std::to_string(EventLog::max_events - 1)); + CHECK_EQ(log.collisions(), 1U); +} +} // namespace +} // namespace micras::sim diff --git a/tests/unit/firmware_thread_test.cpp b/tests/unit/firmware_thread_test.cpp index 4f16257..0bcdc2f 100644 --- a/tests/unit/firmware_thread_test.cpp +++ b/tests/unit/firmware_thread_test.cpp @@ -2,50 +2,46 @@ #include #include -#include +#include #include "micras/sim/core/firmware_thread.hpp" namespace micras::sim { namespace { -TEST(FirmwareThread, RunsTheProgramUpToEachYield) { +TEST_CASE("FirmwareThread.RunsTheProgramUpToEachYield") { std::vector trace; - FirmwareThread* handle = nullptr; - FirmwareThread firmware([&] { + FirmwareThread firmware([&](FirmwareThread& thread) { for (int step = 0; step < 3; step++) { trace.push_back(step); - handle->yield_tick(); + thread.yield_tick(); } }); - handle = &firmware; firmware.run_until_yield(); - EXPECT_EQ(trace, (std::vector{0})); + CHECK_EQ(trace, (std::vector{0})); firmware.run_until_yield(); - EXPECT_EQ(trace, (std::vector{0, 1})); + CHECK_EQ(trace, (std::vector{0, 1})); firmware.run_until_yield(); - EXPECT_EQ(trace, (std::vector{0, 1, 2})); + CHECK_EQ(trace, (std::vector{0, 1, 2})); } -TEST(FirmwareThread, NeverRunsBesideTheSimulation) { +TEST_CASE("FirmwareThread.NeverRunsBesideTheSimulation") { std::atomic awake{0}; std::atomic overlaps{0}; - FirmwareThread* handle = nullptr; - FirmwareThread firmware([&] { + FirmwareThread firmware([&](FirmwareThread& thread) { while (true) { if (awake.fetch_add(1) != 0) { overlaps++; } awake--; - handle->yield_tick(); + thread.yield_tick(); } }); - handle = &firmware; for (int tick = 0; tick < 200; tick++) { firmware.run_until_yield(); @@ -57,7 +53,7 @@ TEST(FirmwareThread, NeverRunsBesideTheSimulation) { awake--; } - EXPECT_EQ(overlaps, 0); + CHECK_EQ(overlaps, 0); } /** @@ -78,26 +74,24 @@ class UnwindSentinel { bool& flag; // NOLINT(*-avoid-const-or-ref-data-members): the test owns the flag. }; -TEST(FirmwareThread, EndlessProgramsAreUnwoundByFinish) { - bool destructor_ran = false; - FirmwareThread* handle = nullptr; +TEST_CASE("FirmwareThread.EndlessProgramsAreUnwoundByFinish") { + bool destructor_ran = false; { - FirmwareThread firmware([&] { + FirmwareThread firmware([&](FirmwareThread& thread) { const UnwindSentinel sentinel(destructor_ran); while (true) { - handle->yield_tick(); + thread.yield_tick(); } }); - handle = &firmware; firmware.run_until_yield(); - EXPECT_FALSE(firmware.has_finished()); + CHECK_FALSE(firmware.has_finished()); firmware.finish(); } - EXPECT_TRUE(destructor_ran); + CHECK(destructor_ran); } /** @@ -122,47 +116,45 @@ class YieldingSentinel { bool& flag; // NOLINT(*-avoid-const-or-ref-data-members): the test owns the flag. }; -TEST(FirmwareThread, UnwindingIsNotBlockedByAYieldingDestructor) { - FirmwareThread* handle = nullptr; - bool destructor_ran = false; +TEST_CASE("FirmwareThread.UnwindingIsNotBlockedByAYieldingDestructor") { + bool destructor_ran = false; - FirmwareThread firmware([&] { - const YieldingSentinel sentinel(*handle, destructor_ran); + FirmwareThread firmware([&](FirmwareThread& thread) { + const YieldingSentinel sentinel(thread, destructor_ran); while (true) { - handle->yield_tick(); + thread.yield_tick(); } }); - handle = &firmware; firmware.run_until_yield(); firmware.finish(); - EXPECT_TRUE(destructor_ran); + CHECK(destructor_ran); } -TEST(FirmwareThread, ReportsAProgramThatReturns) { - FirmwareThread firmware([] {}); +TEST_CASE("FirmwareThread.ReportsAProgramThatReturns") { + FirmwareThread firmware([](FirmwareThread&) { }); firmware.run_until_yield(); - EXPECT_TRUE(firmware.has_finished()); - EXPECT_NO_THROW(firmware.rethrow_any_error()); + CHECK(firmware.has_finished()); + CHECK_NOTHROW(firmware.rethrow_any_error()); } -TEST(FirmwareThread, CarriesAProgramErrorBackToTheSimulation) { - FirmwareThread firmware([] { throw std::runtime_error("the firmware gave up"); }); +TEST_CASE("FirmwareThread.CarriesAProgramErrorBackToTheSimulation") { + FirmwareThread firmware([](FirmwareThread&) { throw std::runtime_error("the firmware gave up"); }); firmware.run_until_yield(); - EXPECT_TRUE(firmware.has_finished()); - EXPECT_THROW(firmware.rethrow_any_error(), std::runtime_error); + CHECK(firmware.has_finished()); + CHECK_THROWS_AS(firmware.rethrow_any_error(), std::runtime_error); } -TEST(FirmwareThread, RunningAFinishedProgramIsHarmless) { - FirmwareThread firmware([] {}); +TEST_CASE("FirmwareThread.RunningAFinishedProgramIsHarmless") { + FirmwareThread firmware([](FirmwareThread&) { }); firmware.run_until_yield(); - EXPECT_NO_THROW(firmware.run_until_yield()); - EXPECT_NO_THROW(firmware.finish()); + CHECK_NOTHROW(firmware.run_until_yield()); + CHECK_NOTHROW(firmware.finish()); } } // namespace } // namespace micras::sim diff --git a/tests/unit/maze_test.cpp b/tests/unit/maze_test.cpp new file mode 100644 index 0000000..8146405 --- /dev/null +++ b/tests/unit/maze_test.cpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "micras/sim/arenas/maze.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/span_at.hpp" +#include "micras/sim/core/text_file.hpp" + +namespace micras::sim { +namespace { +/** + * @brief A two by two maze: the start at the lower left, a goal above it, one wall inside each row. + */ +constexpr std::string_view small_maze{"o---o---o\n" + "| G |\n" + "o o---o\n" + "| S | |\n" + "o---o---o\n"}; + +TEST_CASE("Maze.ReadsTheSizeOfTheDrawing") { + const Maze maze = Maze::parse(small_maze); + + CHECK_EQ(maze.width(), 2U); + CHECK_EQ(maze.height(), 2U); +} + +TEST_CASE("Maze.ReadsTheWallsBottomUp") { + const Maze maze = Maze::parse(small_maze); + + CHECK(maze.south_wall(0, 0)); + CHECK(maze.south_wall(1, 0)); + CHECK_FALSE(maze.south_wall(0, 1)); + CHECK(maze.south_wall(1, 1)); + CHECK(maze.south_wall(0, 2)); + CHECK(maze.south_wall(1, 2)); + + CHECK(maze.west_wall(0, 0)); + CHECK(maze.west_wall(1, 0)); + CHECK(maze.west_wall(2, 0)); + CHECK(maze.west_wall(0, 1)); + CHECK_FALSE(maze.west_wall(1, 1)); + CHECK(maze.west_wall(2, 1)); +} + +TEST_CASE("Maze.ReportsNoWallOutsideTheDrawing") { + const Maze maze = Maze::parse(small_maze); + + CHECK_FALSE(maze.south_wall(2, 0)); + CHECK_FALSE(maze.south_wall(0, 3)); + CHECK_FALSE(maze.west_wall(3, 0)); + CHECK_FALSE(maze.west_wall(0, 2)); +} + +TEST_CASE("Maze.ReadsTheStartAndTheGoals") { + const Maze maze = Maze::parse(small_maze); + + CHECK_EQ(maze.start(), (std::pair{0, 0})); + REQUIRE_EQ(maze.goals().size(), 1U); + CHECK_EQ(maze.goals().front(), (std::pair{0, 1})); +} + +TEST_CASE("Maze.StartsAtTheLowerLeftWhenTheDrawingMarksNoStart") { + const Maze maze = Maze::parse("o---o---o\n| | S |\no---o---o\n"); + const Maze unmarked = Maze::parse("o---o---o\n| |\no---o---o\n"); + + CHECK_EQ(maze.start(), (std::pair{1, 0})); + CHECK_EQ(unmarked.start(), (std::pair{0, 0})); + CHECK(unmarked.goals().empty()); +} + +TEST_CASE("Maze.IgnoresTrailingSpacesCarriageReturnsAndBlankLines") { + const Maze maze = Maze::parse("o---o---o \r\n| G |\r\no o---o\r\n| S | | \r\no---o---o\r\n\n\n"); + + CHECK_EQ(maze.width(), 2U); + CHECK_EQ(maze.height(), 2U); + CHECK_FALSE(maze.west_wall(1, 1)); + CHECK(maze.west_wall(1, 0)); + CHECK_EQ(maze.goals().size(), 1U); +} + +TEST_CASE("Maze.RejectsADrawingWithAnEvenNumberOfLines") { + CHECK_THROWS_AS(Maze::parse("o---o\n| |\no---o\n| |\n"), std::runtime_error); +} + +TEST_CASE("Maze.RejectsADrawingTooShortToHoldACell") { + CHECK_THROWS_AS(Maze::parse(""), std::runtime_error); + CHECK_THROWS_AS(Maze::parse("o---o\n"), std::runtime_error); + CHECK_THROWS_AS(Maze::parse("o\n|\no\n"), std::runtime_error); +} + +TEST_CASE("Maze.RefusesAFileThatDoesNotExist") { + CHECK_THROWS_AS(Maze::load(std::filesystem::path{MICRAS_TEST_MAZES_DIR} / "no_such_maze.txt"), std::runtime_error); +} + +TEST_CASE("Maze.LoadsABundledMazeClosedAllAround") { + const Maze maze = Maze::load(std::filesystem::path{MICRAS_TEST_MAZES_DIR} / "maze1.txt"); + + REQUIRE_EQ(maze.width(), 16U); + REQUIRE_EQ(maze.height(), 16U); + CHECK_EQ(maze.start(), (std::pair{0, 0})); + CHECK_FALSE(maze.goals().empty()); + + for (std::size_t i = 0; i < 16; i++) { + CHECK_MESSAGE(maze.south_wall(i, 0), "bottom edge, column " << i); + CHECK_MESSAGE(maze.south_wall(i, 16), "top edge, column " << i); + CHECK_MESSAGE(maze.west_wall(0, i), "left edge, row " << i); + CHECK_MESSAGE(maze.west_wall(16, i), "right edge, row " << i); + } +} + +TEST_CASE("Maze.GeneratesAnArenaAWorldCanAttach") { + const Maze maze = Maze::parse(small_maze); + MujocoWorld world; + + world.build( + read_text_file(MICRAS_TEST_MODEL, "model"), "robot", maze.mjcf({}), Maze::body_name, {.x = 0.09, .y = 0.09} + ); + + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "maze_floor")); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "maze_south_1_1")); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "maze_west_1_0")); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "maze_post_2_2")); + CHECK_THROWS_AS(world.require_id(mjOBJ_GEOM, "maze_west_1_1"), std::runtime_error); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "chassis")); + CHECK_FALSE(world.composed_xml().empty()); +} + +TEST_CASE("Maze.PlacesTheWallsOnTheCellEdges") { + const Maze maze = Maze::parse(small_maze); + const MazeConfig config{.cell_size = 0.2}; + MujocoWorld world; + + world.build( + read_text_file(MICRAS_TEST_MODEL, "model"), "robot", maze.mjcf(config), Maze::body_name, {.x = 0.1, .y = 0.1} + ); + world.reset(); + + const std::span positions( + world.data()->geom_xpos, static_cast(3 * world.model()->ngeom) + ); + const auto first = static_cast(3 * world.require_id(mjOBJ_GEOM, "maze_south_1_1")); + + CHECK_LE(std::abs(at(positions, first) - 0.3), 1e-9); + CHECK_LE(std::abs(at(positions, first + 1) - 0.2), 1e-9); + CHECK_LE(std::abs(at(positions, first + 2) - (config.wall_height / 2)), 1e-9); +} + +TEST_CASE("Maze.SeesTheFloorDarkAndEverythingElseAsWall") { + const MazeConfig config{.wall_reflectance = 0.7, .floor_reflectance = 0.1}; + + CHECK_EQ(Maze::reflectance("maze_floor", config), doctest::Approx(0.1).epsilon(1e-12)); + CHECK_EQ(Maze::reflectance("floor", config), doctest::Approx(0.1).epsilon(1e-12)); + CHECK_EQ(Maze::reflectance("maze_south_0_0", config), doctest::Approx(0.7).epsilon(1e-12)); + CHECK_EQ(Maze::reflectance("maze_post_1_1", config), doctest::Approx(0.7).epsilon(1e-12)); +} +} // namespace +} // namespace micras::sim diff --git a/tests/unit/mujoco_world_test.cpp b/tests/unit/mujoco_world_test.cpp new file mode 100644 index 0000000..166da77 --- /dev/null +++ b/tests/unit/mujoco_world_test.cpp @@ -0,0 +1,85 @@ +#include +#include +#include +#include + +#include +#include + +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/span_at.hpp" +#include "temp_file.hpp" + +namespace micras::sim { +namespace { +constexpr double gravity{9.81}; +constexpr double coefficient{0.15}; + +constexpr const char* sled_xml = R"( + +)"; + +class Sled { +protected: + Sled() { + std::ofstream(this->file.path()) << sled_xml; + this->world.load(this->file.path()); + this->world.reset(); + this->world.step(800); + } + + double& forward_speed() { + const std::span velocities(this->world.data()->qvel, static_cast(this->world.model()->nv)); + return at(velocities, 0); + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + TempFile file{"sled.xml"}; + MujocoWorld world; + // NOLINTEND(*-non-private-member-variables-in-classes) +}; +} // namespace + +TEST_CASE_FIXTURE(Sled, "A frictionless skid keeps sliding") { + this->forward_speed() = 1.0; + this->world.step(1600); + + CHECK(this->forward_speed() == doctest::Approx(1.0).epsilon(1.0e-6)); +} + +TEST_CASE_FIXTURE(Sled, "A skid with a sliding friction slows down at its coefficient times gravity") { + this->world.add_sliding_friction("skid", coefficient); + this->forward_speed() = 1.0; + this->world.step(1600); + + CHECK(this->forward_speed() == doctest::Approx(1.0 - (coefficient * gravity * 0.2)).epsilon(0.01)); +} + +TEST_CASE_FIXTURE(Sled, "A skid with a sliding friction comes to rest and stays there") { + this->world.add_sliding_friction("skid", coefficient); + this->forward_speed() = 1.0; + this->world.step(8000); + + CHECK(std::abs(this->forward_speed()) < 1.0e-4); +} + +TEST_CASE_FIXTURE(Sled, "Loading another model forgets the sliding frictions") { + this->world.add_sliding_friction("skid", coefficient); + this->world.load(this->file.path()); + this->world.reset(); + this->world.step(800); + this->forward_speed() = 1.0; + this->world.step(1600); + + CHECK(this->forward_speed() == doctest::Approx(1.0).epsilon(1.0e-6)); +} +} // namespace micras::sim diff --git a/tests/unit/noise_test.cpp b/tests/unit/noise_test.cpp new file mode 100644 index 0000000..2ab2659 --- /dev/null +++ b/tests/unit/noise_test.cpp @@ -0,0 +1,87 @@ +#include +#include +#include +#include + +#include + +#include "micras/sim/core/noise.hpp" + +namespace micras::sim { +/** + * @brief Draw a number of samples from a fresh stream. + * + * @param config The run's noise settings. + * @param stream Name of the stream. + * @param count Number of samples. + * @param sigma Standard deviation of every sample. + * @return The samples. + */ +static std::vector + draw(const NoiseConfig& config, std::string_view stream, std::size_t count, double sigma = 1.0) { + Noise noise(config, stream); + std::vector samples; + samples.reserve(count); + + for (std::size_t i = 0; i < count; i++) { + samples.push_back(noise.gaussian(sigma)); + } + + return samples; +} + +namespace { +TEST_CASE("Noise.RepeatsAStreamForTheSameSeedAndName") { + CHECK_EQ(draw({.seed = 3}, "gyro", 100), draw({.seed = 3}, "gyro", 100)); +} + +TEST_CASE("Noise.GivesEachNameItsOwnStream") { + CHECK_NE(draw({.seed = 3}, "gyro", 100), draw({.seed = 3}, "accelerometer", 100)); + CHECK_NE(draw({.seed = 3}, "a", 100), draw({.seed = 3}, "b", 100)); +} + +TEST_CASE("Noise.GivesEachSeedItsOwnStream") { + CHECK_NE(draw({.seed = 3}, "gyro", 100), draw({.seed = 4}, "gyro", 100)); +} + +TEST_CASE("Noise.ScalesTheSameSamplesBySigma") { + const std::vector unit = draw({.seed = 5}, "gyro", 50); + const std::vector scaled = draw({.seed = 5}, "gyro", 50, 2.5); + + for (std::size_t i = 0; i < unit.size(); i++) { + CHECK_EQ(scaled.at(i), doctest::Approx(2.5 * unit.at(i)).epsilon(1e-12)); + } +} + +TEST_CASE("Noise.DrawsACenteredGaussianOfTheRequestedSpread") { + const std::vector samples = draw({.seed = 11}, "gyro", 20000, 2.0); + double sum = 0.0; + double squares = 0.0; + + for (const double sample : samples) { + sum += sample; + squares += sample * sample; + } + + const auto count = static_cast(samples.size()); + const double mean = sum / count; + + CHECK_LE(std::abs(mean - 0.0), 0.05); + CHECK_LE(std::abs(std::sqrt(squares / count - mean * mean) - 2.0), 0.05); +} + +TEST_CASE("Noise.DrawsNothingInAnIdealWorld") { + Noise noise({.seed = 3, .ideal = true}, "gyro"); + + for (int i = 0; i < 100; i++) { + CHECK_EQ(noise.gaussian(1.0), 0.0); + } +} + +TEST_CASE("Noise.DrawsNothingForAZeroSigma") { + Noise noise({.seed = 3}, "gyro"); + + CHECK_EQ(noise.gaussian(0.0), 0.0); +} +} // namespace +} // namespace micras::sim diff --git a/tests/unit/packet_framer_test.cpp b/tests/unit/packet_framer_test.cpp deleted file mode 100644 index f247a65..0000000 --- a/tests/unit/packet_framer_test.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include - -#include "micras/comm/packet.hpp" -#include "micras/sim/telemetry/packet_framer.hpp" - -namespace micras::sim { -namespace { -std::vector serialized(comm::Packet::MessageType type, uint16_t id, const std::vector& payload) { - return comm::Packet(type, id, payload).serialize(); -} - -TEST(PacketFramer, YieldsOnePacketPerFrame) { - PacketFramer framer; - const auto first = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 3, {1, 2, 3}); - const auto second = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 4, {9}); - - std::vector stream = first; - stream.insert(stream.end(), second.begin(), second.end()); - framer.push(stream); - - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 2U); - EXPECT_EQ(packets[0], first); - EXPECT_EQ(packets[1], second); - EXPECT_TRUE(framer.drain().empty()); -} - -TEST(PacketFramer, KeepsIncompleteTail) { - PacketFramer framer; - const auto packet = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 3, {1, 2, 3}); - - framer.push(std::span(packet).first(packet.size() - 2)); - EXPECT_TRUE(framer.drain().empty()); - - framer.push(std::span(packet).last(2)); - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 1U); - EXPECT_EQ(packets[0], packet); -} - -TEST(PacketFramer, SkipsGarbageBeforeHeader) { - PacketFramer framer; - const auto packet = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 3, {1, 2, 3}); - - std::vector stream{0x00, 0x11, 0x22}; - stream.insert(stream.end(), packet.begin(), packet.end()); - framer.push(stream); - - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 1U); - EXPECT_EQ(packets[0], packet); -} - -TEST(PacketFramer, HandlesControlBytesInsidePayload) { - PacketFramer framer; - const auto packet = serialized( - comm::Packet::MessageType::SERIAL_VARIABLE, comm::Packet::tail_byte, - {comm::Packet::tail_byte, comm::Packet::escape_byte, comm::Packet::header_byte} - ); - framer.push(packet); - - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 1U); - EXPECT_EQ(packets[0], packet); - EXPECT_EQ(comm::Packet(packets[0]).get_payload(), (std::vector{0x7F, 0x7D, 0x42})); -} - -TEST(PacketFramer, HandlesControlBytesInTheLengthField) { - for (const std::size_t length : {std::size_t{comm::Packet::escape_byte}, std::size_t{comm::Packet::tail_byte}}) { - PacketFramer framer; - const auto packet = - serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 1, std::vector(length, 0x01)); - framer.push(packet); - - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 1U) << "payload length " << length; - EXPECT_EQ(packets[0], packet); - } -} - -TEST(PacketFramer, FramesAPacketPushedOneByteAtATime) { - PacketFramer framer; - const auto packet = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 3, {0x7F, 0x7D, 0x42}); - - for (std::size_t i = 0; i + 1 < packet.size(); i++) { - framer.push(std::span(packet).subspan(i, 1)); - EXPECT_TRUE(framer.drain().empty()) << "framed early at byte " << i; - } - - framer.push(std::span(packet).last(1)); - const auto packets = framer.drain(); - ASSERT_EQ(packets.size(), 1U); - EXPECT_EQ(packets[0], packet); -} - -TEST(PacketFramer, ResyncsWhenNoFrameCloses) { - PacketFramer framer; - const std::vector noise(PacketFramer::max_buffer_size + 1, comm::Packet::header_byte); - framer.push(noise); - - EXPECT_TRUE(framer.drain().empty()); - EXPECT_EQ(framer.resync_count(), 1U); -} - -TEST(PacketFramer, FramesAgainAfterAResync) { - PacketFramer framer; - const std::vector noise(PacketFramer::max_buffer_size + 1, comm::Packet::header_byte); - framer.push(noise); - EXPECT_TRUE(framer.drain().empty()); - - const auto packet = serialized(comm::Packet::MessageType::SERIAL_VARIABLE, 3, {1, 2, 3}); - framer.push(packet); - - const auto packets = framer.drain(); - ASSERT_FALSE(packets.empty()); - EXPECT_EQ(packets.back(), packet); -} -} // namespace -} // namespace micras::sim diff --git a/tests/unit/proxy_boundary_test.cpp b/tests/unit/proxy_boundary_test.cpp deleted file mode 100644 index b1e7582..0000000 --- a/tests/unit/proxy_boundary_test.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include - -#include "micras/proxy/bluetooth_serial.hpp" -#include "micras/proxy/button.hpp" -#include "micras/proxy/dip_switch.hpp" -#include "micras/proxy/fan.hpp" -#include "micras/proxy/imu.hpp" -#include "micras/proxy/locomotion.hpp" -#include "micras/proxy/rotary_sensor.hpp" -#include "micras/proxy/stopwatch.hpp" -#include "micras/proxy/wall_sensors.hpp" -#include "micras/sim/core/simulation_context.hpp" - -namespace micras::proxy { -namespace { -/** - * @brief A simulation of its own, as Appendix A of the plan requires of tests. - * - * @note Nothing here touches sim::SimulationContext::instance(): the proxies - * are handed this context through their Config, exactly as target.hpp - * hands them the process-wide one. - * - * @note SetUp advances the dynamics by one tick before any test runs, because - * the accelerometer and every contact derived sensor only carry a valid - * reading once they have, exactly as in a run. - */ -/** - * @brief Ticks the robot needs to fall the spawn gap and land on its wheels. - * - * @note Until it lands the accelerometer reads free fall, so a test that wants - * to see gravity has to wait out the drop first. - */ -constexpr int ticks_until_the_wheels_land{200}; - -class ProxyBoundary : public testing::Test { -protected: - void SetUp() override { - this->context.world.load(MICRAS_TEST_MODEL); - this->context.clock = sim::Clock::from_model(this->context.world.timestep(), 1042); - this->context.world.reset(); - - this->context.world.step(this->context.clock.steps_per_tick()); - } - - // NOLINTNEXTLINE(*-non-private-member-variables-in-classes): the fixture is the test's own scope. - sim::SimulationContext context; -}; - -TEST_F(ProxyBoundary, ProxiesRefuseToBuildWithoutAContext) { - EXPECT_THROW(Imu({.gyro_sensor = "gyro", .accelerometer_sensor = "accelerometer"}), std::logic_error); - EXPECT_THROW(Motor({.actuator = "motor_left"}), std::logic_error); -} - -TEST_F(ProxyBoundary, ProxiesRefuseToBuildWhenTheModelLacksTheObject) { - EXPECT_THROW(Motor({.actuator = "no_such_actuator", .context = &this->context}), std::runtime_error); -} - -TEST_F(ProxyBoundary, TheImuPublishesItsReadings) { - Imu imu({.gyro_sensor = "gyro", .accelerometer_sensor = "accelerometer", .context = &this->context}); - - for (int tick = 0; tick < ticks_until_the_wheels_land; tick++) { - this->context.world.step(this->context.clock.steps_per_tick()); - } - - imu.update(); - - const float published_rate = this->context.proxy_state.sensors.angular_velocity.at(2); - const float published_acceleration = this->context.proxy_state.sensors.linear_acceleration.at(2); - - EXPECT_FLOAT_EQ(published_rate, imu.get_angular_velocity(Imu::Axis::Z)); - EXPECT_FLOAT_EQ(published_acceleration, imu.get_linear_acceleration(Imu::Axis::Z)); - EXPECT_NEAR(published_acceleration, 9.81F, 0.05F); -} - -TEST_F(ProxyBoundary, EachEncoderPublishesIntoItsOwnWheel) { - const RotarySensor left({.sensor = "encoder_left", .wheel = 0, .context = &this->context}); - const RotarySensor right({.sensor = "encoder_right", .wheel = 1, .context = &this->context}); - - const float left_position = left.get_position(); - const float right_position = right.get_position(); - - EXPECT_FLOAT_EQ(this->context.proxy_state.sensors.encoder_positions.at(0), left_position); - EXPECT_FLOAT_EQ(this->context.proxy_state.sensors.encoder_positions.at(1), right_position); -} - -TEST_F(ProxyBoundary, TheWallSensorsPublishTheirUnfilteredReadings) { - const TWallSensors<4> sensors({ - .sensors = {"lidar_0", "lidar_1", "lidar_2", "lidar_3"}, - .uncertainty = 0.5F, - .base_readings = {0.0696F, 0.1090F, 0.1090F, 0.0696F}, - .max_sensor_reading = 0.6F, - .min_sensor_reading = 0.01F, - .max_sensor_distance = 0.36F, - .filter_cutoff = 30.55F, - .sampling_frequency = 959.69F, - .context = &this->context, - }); - - for (uint8_t i = 0; i < 4; i++) { - const float reading = sensors.get_adc_reading(i); - - EXPECT_GT(reading, 0.0F) << +i; - EXPECT_FLOAT_EQ(this->context.proxy_state.sensors.wall_adc_readings.at(i), reading) << +i; - } -} - -TEST_F(ProxyBoundary, LocomotionPublishesTheCommandsItPassesOn) { - Locomotion locomotion({ - .left_motor = {.actuator = "motor_left", .context = &this->context}, - .right_motor = {.actuator = "motor_right", .context = &this->context}, - .context = &this->context, - }); - - locomotion.enable(); - locomotion.set_wheel_command(30.0F, -20.0F); - - EXPECT_FLOAT_EQ(this->context.proxy_state.actuators.left_command, 30.0F); - EXPECT_FLOAT_EQ(this->context.proxy_state.actuators.right_command, -20.0F); - - locomotion.disable(); - locomotion.set_wheel_command(30.0F, -20.0F); - - EXPECT_FLOAT_EQ(this->context.proxy_state.actuators.left_command, 0.0F); - EXPECT_FLOAT_EQ(this->context.proxy_state.actuators.right_command, 0.0F); -} - -TEST_F(ProxyBoundary, TheFanHonoursTheHarnessOverride) { - Fan fan({.actuator = "fan", .max_acceleration = 100.0F, .context = &this->context}); - - fan.set_speed(50.0F); - this->context.clock.advance(); - fan.update(); - EXPECT_GT(this->context.proxy_state.actuators.fan_speed, 0.0F); - - this->context.proxy_state.overrides.fan_enabled = false; - fan.update(); - EXPECT_FLOAT_EQ(this->context.proxy_state.actuators.fan_speed, 0.0F); -} - -TEST_F(ProxyBoundary, TheInterfaceProxiesReadTheStateTheScenarioWrites) { - const Button button({.name = "button", .context = &this->context}); - const TDipSwitch<4> switches({.names = {"a", "b", "c", "d"}, .context = &this->context}); - - this->context.proxy_state.interface_input.button_pressed = true; - this->context.proxy_state.interface_input.dip_switches = {true, false, true, false}; - - EXPECT_FALSE(button.is_pressed()); - EXPECT_TRUE(switches.get_switch_state(0)); - EXPECT_FALSE(switches.get_switch_state(1)); - EXPECT_EQ(switches.get_switches_value(), 0b0101); -} - -TEST_F(ProxyBoundary, AFreshStopwatchReadsTheNominalPeriod) { - const Stopwatch stopwatch({.context = &this->context}); - - EXPECT_EQ(stopwatch.elapsed_time_us(), 1042U); -} - -TEST_F(ProxyBoundary, TheStopwatchRunsOnTheContextClock) { - Stopwatch stopwatch({.context = &this->context}); - - stopwatch.reset_us(); - this->context.clock.advance(); - this->context.clock.advance(); - - EXPECT_EQ(stopwatch.elapsed_time_ms(), 2U * 1042U / 1000U); - EXPECT_EQ(stopwatch.elapsed_time_us(), 2U * 1042U); - - stopwatch.reset_ms(); - EXPECT_EQ(stopwatch.elapsed_time_ms(), 0U); -} - -TEST_F(ProxyBoundary, TheStopwatchDoesNotYieldWithoutAProgram) { - const Stopwatch stopwatch({.context = &this->context}); - - EXPECT_EQ(this->context.firmware.load(), nullptr); - EXPECT_NO_THROW(static_cast(stopwatch.elapsed_time_us())); -} - -TEST_F(ProxyBoundary, TheRadioCarriesBytesBothWays) { - BluetoothSerial radio({.context = &this->context}); - - this->context.serial.queue_for_firmware(std::vector{1, 2, 3}); - EXPECT_EQ(radio.get_data(), (std::vector{1, 2, 3})); - EXPECT_TRUE(radio.get_data().empty()); -} -} // namespace -} // namespace micras::proxy diff --git a/tests/unit/robot_description_test.cpp b/tests/unit/robot_description_test.cpp new file mode 100644 index 0000000..975618e --- /dev/null +++ b/tests/unit/robot_description_test.cpp @@ -0,0 +1,216 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "micras/sim/arenas/maze.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/robot/robot_description.hpp" +#include "micras/sim/robot/robot_model.hpp" + +namespace micras::sim { +/** + * @brief Read the tiny robot's description as text. + * + * @return Contents of tiny_robot.toml. + */ +static std::string tiny_text() { + const std::ifstream file(MICRAS_TEST_ROBOT); + std::ostringstream text; + text << file.rdbuf(); + return text.str(); +} + +/** + * @brief Replace the one occurrence of a piece of text, failing the test when it is not there. + * + * @param text Text to edit. + * @param from Text to replace. + * @param to Replacement. + * @return The edited text. + */ +static std::string replaced(std::string text, std::string_view from, std::string_view to) { + const std::size_t position = text.find(from); + CHECK_MESSAGE(position != std::string::npos, "the tiny robot has no " << from); + + if (position != std::string::npos) { + text.replace(position, from.size(), to); + } + + return text; +} + +/** + * @brief Parse a description and return the message it was refused with. + * + * @param text Contents of a robot.toml. + * @return The error message, or empty when the description was accepted. + */ +static std::string refusal_of(std::string_view text) { + try { + RobotDescription::parse(text, "tiny.toml"); + } catch (const std::runtime_error& error) { + return error.what(); + } + + return {}; +} + +namespace { +TEST_CASE("RobotDescription.ReadsEverySection") { + const RobotDescription robot = RobotDescription::load(MICRAS_TEST_ROBOT); + + CHECK_EQ(robot.name, "tiny"); + CHECK_EQ(robot.integration.timestep, doctest::Approx(0.000521).epsilon(1e-12)); + CHECK_EQ(robot.chassis.mass, doctest::Approx(0.05).epsilon(1e-12)); + CHECK_EQ(robot.chassis.outline.size(), 4U); + CHECK_EQ(robot.wheels.track, doctest::Approx(0.05).epsilon(1e-12)); + CHECK_EQ(robot.drive.resistance(), doctest::Approx(10.5).epsilon(1e-12)); + CHECK_EQ(robot.encoders.counts_per_revolution, 4096U); + CHECK_EQ(robot.imu.axes.at(1).at(1), doctest::Approx(1.0).epsilon(1e-12)); + CHECK_EQ(robot.fan.nominal_voltage, doctest::Approx(7.4).epsilon(1e-12)); + CHECK_EQ(robot.battery.cells, 2); + CHECK_EQ(robot.link.baud_rate, 9600U); + CHECK_EQ(robot.wall_sensors.rays, 4); +} + +TEST_CASE("RobotDescription.ReadsArraysOfParts") { + const RobotDescription robot = RobotDescription::load(MICRAS_TEST_ROBOT); + + REQUIRE_EQ(robot.chassis.boxes.size(), 1U); + CHECK_EQ(robot.chassis.boxes.front().name, "cargo"); + CHECK_EQ(robot.chassis.boxes.front().max.at(2), doctest::Approx(0.02).epsilon(1e-12)); + REQUIRE_EQ(robot.chassis.cylinders.size(), 1U); + CHECK_EQ(robot.chassis.cylinders.front().radius, doctest::Approx(0.005).epsilon(1e-12)); + REQUIRE_EQ(robot.wall_sensors.sensors.size(), 1U); + CHECK_EQ(robot.wall_sensors.sensors.front().name, "eye"); +} + +TEST_CASE("RobotDescription.ConvertsDegreesToRadians") { + const RobotDescription robot = RobotDescription::load(MICRAS_TEST_ROBOT); + + CHECK_EQ(robot.wall_sensors.sensors.front().yaw, doctest::Approx(std::numbers::pi / 2).epsilon(1e-12)); + CHECK_EQ(robot.wall_sensors.emitter_half_angle, doctest::Approx(5.0 * std::numbers::pi / 180.0).epsilon(1e-12)); +} + +TEST_CASE("RobotDescription.AcceptsPlainValuesAndSourcedTablesAlike") { + const std::string text = tiny_text(); + const RobotDescription plain = RobotDescription::parse( + replaced(text, R"(winding_resistance = { value = 10.0, source = "made up" })", "winding_resistance = 10.0") + ); + const RobotDescription sourced = RobotDescription::parse( + replaced(text, "bridge_resistance = 0.5", R"(bridge_resistance = { value = 0.5, source = "datasheet" })") + ); + + CHECK_EQ(plain.drive.winding_resistance, doctest::Approx(10.0).epsilon(1e-12)); + CHECK_EQ(sourced.drive.bridge_resistance, doctest::Approx(0.5).epsilon(1e-12)); + CHECK_EQ(plain.drive.resistance(), doctest::Approx(sourced.drive.resistance()).epsilon(1e-12)); +} + +TEST_CASE("RobotDescription.RefusesAMissingKeyNamingIt") { + const std::string message = refusal_of(replaced(tiny_text(), "bridge_resistance = 0.5\n", "")); + + CHECK_MESSAGE(message.contains("drive.bridge_resistance"), message); + CHECK_MESSAGE(message.contains("missing"), message); +} + +TEST_CASE("RobotDescription.RefusesAMissingSectionNamingIt") { + const std::string message = refusal_of(replaced(tiny_text(), "[link]\nbaud_rate = 9600\n", "")); + + CHECK_MESSAGE(message.contains("[link]"), message); +} + +TEST_CASE("RobotDescription.RefusesAnUnknownKeyNamingIt") { + const std::string message = refusal_of(replaced(tiny_text(), "[battery]\n", "[battery]\ncolor = \"red\"\n")); + + CHECK_MESSAGE(message.contains("unknown key battery.color"), message); +} + +TEST_CASE("RobotDescription.RefusesAnUnknownKeyInAnArrayOfTables") { + const std::string message = refusal_of(replaced(tiny_text(), "name = \"eye\"\n", "name = \"eye\"\nshiny = true\n")); + + CHECK_MESSAGE(message.contains("wall_sensors.sensors[0].shiny"), message); +} + +TEST_CASE("RobotDescription.RefusesAValueOfTheWrongTypeNamingIt") { + const std::string text_instead = refusal_of(replaced(tiny_text(), "cells = 2", "cells = \"two\"")); + const std::string fraction = refusal_of(replaced(tiny_text(), "cells = 2", "cells = 2.5")); + const std::string short_vector = + refusal_of(replaced(tiny_text(), "chip_y = [0.0, 1.0, 0.0]", "chip_y = [0.0, 1.0]")); + const std::string sourced = refusal_of(replaced( + tiny_text(), R"(mass = { value = 0.05, source = "made up" })", R"(mass = { value = "heavy", source = "x" })" + )); + + CHECK_MESSAGE(text_instead.contains("battery.cells must be a whole number"), text_instead); + CHECK_MESSAGE(fraction.contains("battery.cells must be a whole number"), fraction); + CHECK_MESSAGE(short_vector.contains("imu.chip_y must be 3 numbers"), short_vector); + CHECK_MESSAGE(sourced.contains("chassis.mass must be a number"), sourced); +} + +TEST_CASE("RobotDescription.RefusesASourcedTableThatIsNotExactlyAValueAndASource") { + const std::string unsourced = refusal_of(replaced(tiny_text(), R"(, source = "made up" })", " }")); + const std::string extra = + refusal_of(replaced(tiny_text(), R"(source = "made up" })", R"(source = "made up", unit = "kg" })")); + + CHECK_MESSAGE(unsourced.contains("chassis.mass"), unsourced); + CHECK_MESSAGE(extra.contains("chassis.mass"), extra); +} + +TEST_CASE("RobotDescription.RefusesASchemaVersionItDoesNotKnow") { + const std::string message = refusal_of(replaced(tiny_text(), "schema = 1", "schema = 2")); + + CHECK_MESSAGE(message.contains("schema 2"), message); +} + +TEST_CASE("RobotDescription.RefusesAnOutlineOfFewerThanThreePoints") { + const std::string message = refusal_of(replaced( + tiny_text(), "outline = [[0.04, 0.02], [-0.03, 0.02], [-0.03, -0.02], [0.04, -0.02]]", + "outline = [[0.04, 0.02], [-0.03, 0.02]]" + )); + + CHECK_MESSAGE(message.contains("chassis.outline"), message); +} + +TEST_CASE("RobotDescription.NamesTheFileInEveryRefusal") { + const std::string syntax = refusal_of("schema = = 1"); + const std::string missing = refusal_of(replaced(tiny_text(), "bridge_resistance = 0.5\n", "")); + + CHECK_MESSAGE(syntax.starts_with("tiny.toml"), syntax); + CHECK_MESSAGE(missing.starts_with("tiny.toml"), missing); +} + +TEST_CASE("RobotDescription.RefusesAFileThatDoesNotExist") { + CHECK_THROWS_AS( + RobotDescription::load(std::filesystem::path{MICRAS_TEST_ROBOT}.parent_path() / "no_such_robot.toml"), + std::runtime_error + ); +} + +TEST_CASE("RobotDescription.GeneratesAModelThatComposesWithAnArena") { + const RobotDescription robot = RobotDescription::load(MICRAS_TEST_ROBOT); + const RobotModelNames names = RobotModelNames::of(robot); + const Maze maze = Maze::parse("o---o\n| S |\no---o\n"); + MujocoWorld world; + + world.build(robot_mjcf(robot), names.body, maze.mjcf({}), Maze::body_name, {.x = 0.09, .y = 0.09}); + world.reset(); + + CHECK_EQ(names.body, "tiny"); + CHECK_EQ(world.timestep(), doctest::Approx(robot.integration.timestep).epsilon(1e-12)); + CHECK_NOTHROW(world.require_id(mjOBJ_BODY, names.body)); + CHECK_MESSAGE(world.model()->njnt == 3, "the free joint and one per wheel"); + CHECK_MESSAGE(world.model()->nu == 3, "one motor per wheel and the fan"); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "cargo")); + CHECK_NOTHROW(world.require_id(mjOBJ_GEOM, "mast")); + CHECK_NOTHROW(world.require_id(mjOBJ_SITE, "eye_emitter")); + CHECK_NOTHROW(world.sensor_address(names.gyro)); +} +} // namespace +} // namespace micras::sim diff --git a/tests/unit/run_metadata_test.cpp b/tests/unit/run_metadata_test.cpp index 4c63bbd..f33f5d4 100644 --- a/tests/unit/run_metadata_test.cpp +++ b/tests/unit/run_metadata_test.cpp @@ -1,90 +1,111 @@ #include -#include +#include #include #include +#include -#include +#include #include "micras/sim/recording/run_metadata.hpp" +#include "temp_file.hpp" namespace micras::sim { namespace { -TEST(RunMetadata, SerializesInTheBaselineLayout) { +TEST_CASE("RunMetadata.SerializesInTheBaselineLayout") { const RunMetadata metadata{ - .firmware_sha = "ad342541267bb3c8db263630f27fc7edafba5b09", - .model_path = "models/robot_v2.xml", - .model_sha256 = "3ef384df", - .maze_path = "", - .mujoco_version = "3.3.6", - .compiler = "GNU 13.3.0", + .target = "tiny", + .target_dir = "targets/tiny", + .firmware_sha = "0be27df", + .robot_path = "robot.toml", + .robot_sha256 = "a1", + .scenario_path = "idle.toml", + .maze_path = "maze1.txt", + .model_sha256 = "b2", + .mujoco_version = "3.14.0", + .compiler = "GNU 13.4.0", .build_type = "RelWithDebInfo", - .args = "--model models/robot_v2.xml --seconds 4 --out runs/idle", - .loop_time_us = 1042, - .timestep = 0.000521, - .steps_per_tick = 2, - .requested_ticks = 3838, - .ticks = 3838, - .sim_time = 3.9992, - .final_z = 0.000806901, - .pool_columns = 29, - .telemetry_resyncs = 0, + .args = "--out runs/idle", + .seed = 1, + .ideal = false, + .loop_time_us = 125, + .timestep = 0.000125, + .steps_per_tick = 1, + .record_every = 1, + .requested_ticks = 32000, + .ticks = 32000, + .sim_time = 4, + .stopped_at = -1, + .final_z = 0.0001, + .target_fields = {{.name = "unbound_ports", .value = 0}}, .warnings_total = 0, .interactive = false, + .events = {{.time = 0.5, .kind = "state", .detail = "IDLE"}}, }; const std::string expected = "{\n" - " \"firmware_sha\": \"ad342541267bb3c8db263630f27fc7edafba5b09\",\n" - " \"model_path\": \"models/robot_v2.xml\",\n" - " \"model_sha256\": \"3ef384df\",\n" - " \"maze_path\": \"\",\n" - " \"mujoco_version\": \"3.3.6\",\n" - " \"compiler\": \"GNU 13.3.0\",\n" + " \"target\": \"tiny\",\n" + " \"target_dir\": \"targets/tiny\",\n" + " \"firmware_sha\": \"0be27df\",\n" + " \"robot_path\": \"robot.toml\",\n" + " \"robot_sha256\": \"a1\",\n" + " \"scenario_path\": \"idle.toml\",\n" + " \"maze_path\": \"maze1.txt\",\n" + " \"model_sha256\": \"b2\",\n" + " \"mujoco_version\": \"3.14.0\",\n" + " \"compiler\": \"GNU 13.4.0\",\n" " \"build_type\": \"RelWithDebInfo\",\n" - " \"args\": \"--model models/robot_v2.xml --seconds 4 --out runs/idle\",\n" - " \"loop_time_us\": 1042,\n" - " \"timestep\": 0.000521,\n" - " \"steps_per_tick\": 2,\n" - " \"requested_ticks\": 3838,\n" - " \"ticks\": 3838,\n" - " \"sim_time\": 3.9992,\n" - " \"final_z\": 0.000806901,\n" - " \"pool_columns\": 29,\n" - " \"telemetry_resyncs\": 0,\n" + " \"args\": \"--out runs/idle\",\n" + " \"seed\": 1,\n" + " \"ideal\": false,\n" + " \"loop_time_us\": 125,\n" + " \"timestep\": 0.000125,\n" + " \"steps_per_tick\": 1,\n" + " \"record_every\": 1,\n" + " \"requested_ticks\": 32000,\n" + " \"ticks\": 32000,\n" + " \"sim_time\": 4,\n" + " \"stopped_at\": -1,\n" + " \"final_z\": 0.0001,\n" + " \"unbound_ports\": 0,\n" " \"warnings_total\": 0,\n" - " \"interactive\": false\n" + " \"serial_dropped_bytes\": 0,\n" + " \"bridge_dropped_frames\": 0,\n" + " \"interactive\": false,\n" + " \"events\": [\n" + " {\"time\": 0.5, \"kind\": \"state\", \"detail\": \"IDLE\"}\n" + " ]\n" "}\n"; - EXPECT_EQ(metadata.to_json(), expected); + CHECK_EQ(metadata.to_json(), expected); } -TEST(RunMetadata, EscapesJsonStrings) { +TEST_CASE("RunMetadata.EscapesJsonStrings") { RunMetadata metadata; metadata.args = std::string("quote\" backslash\\ newline\n tab\t bell") + '\001'; const std::string json = metadata.to_json(); const std::string expected = R"("args": "quote\" backslash\\ newline\n tab\t bell\u0001")"; - EXPECT_NE(json.find(expected), std::string::npos); + CHECK_NE(json.find(expected), std::string::npos); } -TEST(RunMetadata, HashesFilesWithSha256) { - const std::filesystem::path path = std::filesystem::temp_directory_path() / "micras_sha256_test.txt"; - std::ofstream(path) << "abc"; +TEST_CASE("RunMetadata.HashesFilesWithSha256") { + const TempFile text{"sha256_test.txt"}; + std::ofstream(text.path()) << "abc"; - EXPECT_EQ(RunMetadata::sha256_of(path), "ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad"); - EXPECT_EQ(RunMetadata::sha256_of("/nonexistent/file"), ""); - std::filesystem::remove(path); + CHECK_EQ(RunMetadata::sha256_of(text.path()), "ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad"); + CHECK_EQ(RunMetadata::sha256_of("/nonexistent/file"), ""); } -TEST(RunMetadata, JoinsArgumentsAfterTheProgramName) { - std::array arguments{"bin", "--model", "m.xml", "--out"}; +TEST_CASE("RunMetadata.JoinsArgumentsAfterTheProgramName") { + std::array arguments{"bin", "--maze", "maze1", "--out"}; std::array argv{}; for (std::size_t i = 0; i < arguments.size(); i++) { argv.at(i) = arguments.at(i).data(); } - EXPECT_EQ(RunMetadata::join_args(argv), "--model m.xml --out"); - EXPECT_EQ(RunMetadata::join_args(std::span(argv).first(1)), ""); + CHECK_EQ(RunMetadata::join_args(argv), "--maze maze1 --out"); + CHECK_EQ(RunMetadata::join_args(std::span(argv).first(1)), ""); } } // namespace } // namespace micras::sim diff --git a/tests/unit/scenario_test.cpp b/tests/unit/scenario_test.cpp index 9b7ac73..20d0d1f 100644 --- a/tests/unit/scenario_test.cpp +++ b/tests/unit/scenario_test.cpp @@ -1,38 +1,439 @@ -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#include "micras/sim/core/scenario.hpp" +#include + +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/simulation.hpp" +#include "micras/sim/core/variable_source.hpp" +#include "micras/sim/devices/digital_input.hpp" +#include "micras/sim/scenario/scenario.hpp" +#include "support.hpp" namespace micras::sim { namespace { -constexpr ButtonDelays firmware_delays{.long_press = 500, .extra_long_press = 2000}; - -TEST(Scenario, ParsesTheNamesTheCommandLineUses) { - EXPECT_EQ(parse_command("none"), Command::NONE); - EXPECT_EQ(parse_command("explore"), Command::EXPLORE); - EXPECT_EQ(parse_command("solve"), Command::SOLVE); - EXPECT_EQ(parse_command("calibrate"), Command::CALIBRATE); - EXPECT_EQ(parse_command("Explore"), std::nullopt); - EXPECT_EQ(parse_command(""), std::nullopt); - - EXPECT_EQ(parse_button_press("none"), ButtonPress::NONE); - EXPECT_EQ(parse_button_press("short"), ButtonPress::SHORT); - EXPECT_EQ(parse_button_press("long"), ButtonPress::LONG); - EXPECT_EQ(parse_button_press("extra_long"), ButtonPress::EXTRA_LONG); - EXPECT_EQ(parse_button_press("extralong"), std::nullopt); -} - -TEST(Scenario, AddressesEachCommandThroughItsPoolVariable) { - EXPECT_EQ(command_variable(Command::EXPLORE), "Explore"); - EXPECT_EQ(command_variable(Command::SOLVE), "Solve"); - EXPECT_EQ(command_variable(Command::CALIBRATE), "Calibrate"); - EXPECT_TRUE(command_variable(Command::NONE).empty()); -} - -TEST(Scenario, HoldsTheButtonPastEachClassificationThreshold) { - EXPECT_EQ(Scenario::hold_time_ms(ButtonPress::NONE, firmware_delays), 0U); - EXPECT_EQ(Scenario::hold_time_ms(ButtonPress::SHORT, firmware_delays), 250U); - EXPECT_EQ(Scenario::hold_time_ms(ButtonPress::LONG, firmware_delays), 501U); - EXPECT_EQ(Scenario::hold_time_ms(ButtonPress::EXTRA_LONG, firmware_delays), 2001U); +/** + * @brief A scenario that uses every section of the format, its events out of order. + */ +constexpr std::string_view full_scenario{R"( +robot = "tiny" +arena = "maze1" +seconds = 12 +seed = 7 + +[start] +x = 0.09 +y = 0.18 +yaw_deg = 90 + +[[events]] +at = 2.0 +send = "hello" + +[[events]] +at = 0.5 +press = "button" +for = 0.25 + +[[events]] +at = 1.0 +set = "switch" +value = true + +[stop] +when = "state" +equals = "IDLE" +after = 5.0 +)"}; + +/** + * @brief A variable that takes a new value on given ticks, updated before the player reads it. + */ +class SteppedVariables : public VariableSource, public IRunListener { +public: + SteppedVariables(std::string name, std::map steps) : + name{std::move(name)}, steps{std::move(steps)} { } + + double value_of(const std::string& name) const override { + return name == this->name ? this->current : std::nan(""); + } + + RunControl on_before_tick(const Simulation& simulation) override { + const auto step = this->steps.find(simulation.tick()); + + if (step != this->steps.end()) { + this->current = step->second; + } + + return RunControl::RUN; + } + +private: + std::string name; + std::map steps; + double current{std::nan("")}; +}; + +/** + * @brief Records, before every tick, what the scenario has done so far. + */ +class Probe : public IRunListener { +public: + explicit Probe(const DigitalInput& input) : input{input} { } + + RunControl on_before_tick(const Simulation& simulation) override { + if (this->input.is_active()) { + this->pressed_ticks.push_back(simulation.tick()); + } + + return RunControl::RUN; + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the test reads what the probe saw. + std::vector pressed_ticks; + // NOLINTEND(*-non-private-member-variables-in-classes) + +private: + const DigitalInput& input; // NOLINT(*-avoid-const-or-ref-data-members): the fixture outlives the probe. +}; +} // namespace + +/** + * @brief Parse a scenario and return the message it was refused with. + * + * @param text Contents of a scenario file. + * @return The error message, or empty when the scenario was accepted. + */ +static std::string refusal_of(std::string_view text) { + try { + Scenario::parse(text, "test.toml"); + } catch (const std::runtime_error& error) { + return error.what(); + } + + return {}; +} + +namespace { +TEST_CASE("Scenario.ReadsTheRunSettings") { + const Scenario scenario = Scenario::parse(full_scenario); + + CHECK_EQ(scenario.robot, "tiny"); + CHECK_EQ(scenario.arena, "maze1"); + CHECK_EQ(scenario.seconds, doctest::Approx(12.0).epsilon(1e-12)); + CHECK_EQ(scenario.seed, 7U); + REQUIRE(scenario.start.has_value()); + + const MujocoWorld::Placement start = scenario.start.value_or(MujocoWorld::Placement{}); + CHECK_EQ(start.x, doctest::Approx(0.09).epsilon(1e-12)); + CHECK_EQ(start.y, doctest::Approx(0.18).epsilon(1e-12)); + CHECK_EQ(start.yaw, doctest::Approx(std::numbers::pi / 2).epsilon(1e-12)); +} + +TEST_CASE("Scenario.LeavesOutWhatTheFileDoesNotSay") { + const Scenario scenario = Scenario::parse("seconds = 4.5\n"); + + CHECK(scenario.robot.empty()); + CHECK(scenario.arena.empty()); + CHECK_EQ(scenario.seconds, doctest::Approx(4.5).epsilon(1e-12)); + CHECK_FALSE(scenario.seed.has_value()); + CHECK_FALSE(scenario.start.has_value()); + CHECK(scenario.events.empty()); + CHECK_FALSE(scenario.stop.has_value()); +} + +TEST_CASE("Scenario.ReadsEveryKindOfEventInTimeOrder") { + const Scenario scenario = Scenario::parse(full_scenario); + + REQUIRE_EQ(scenario.events.size(), 3U); + + const ScenarioEvent& press = scenario.events.at(0); + CHECK_EQ(press.kind, ScenarioEvent::Kind::PRESS); + CHECK_EQ(press.at, doctest::Approx(0.5).epsilon(1e-12)); + CHECK_EQ(press.duration, doctest::Approx(0.25).epsilon(1e-12)); + CHECK_EQ(press.target, "button"); + + const ScenarioEvent& set = scenario.events.at(1); + CHECK_EQ(set.kind, ScenarioEvent::Kind::SET); + CHECK_EQ(set.target, "switch"); + CHECK(set.value); + + const ScenarioEvent& send = scenario.events.at(2); + CHECK_EQ(send.kind, ScenarioEvent::Kind::SEND); + CHECK_EQ(send.target, "hello"); +} + +TEST_CASE("Scenario.ReadsTheStopConditionAsANameOrANumber") { + const Scenario named = Scenario::parse(full_scenario); + const Scenario numbered = Scenario::parse("[stop]\nwhen = \"lap\"\nequals = 3\n"); + + REQUIRE(named.stop.has_value()); + REQUIRE(numbered.stop.has_value()); + + const StopCondition by_name = named.stop.value_or(StopCondition{}); + const StopCondition by_number = numbered.stop.value_or(StopCondition{}); + CHECK_EQ(by_name.variable, "state"); + CHECK_EQ(by_name.equals, std::vector{"IDLE"}); + CHECK_EQ(by_name.after, doctest::Approx(5.0).epsilon(1e-12)); + CHECK_EQ(by_number.equals, std::vector{"3"}); + CHECK_EQ(by_number.after, doctest::Approx(0.0).epsilon(1e-12)); +} + +TEST_CASE("Scenario.ReadsAStopConditionWithSeveralValues") { + const Scenario scenario = Scenario::parse("[stop]\nwhen = \"state\"\nequals = [\"IDLE\", 2]\n"); + + REQUIRE(scenario.stop.has_value()); + CHECK_EQ(scenario.stop.value_or(StopCondition{}).equals, (std::vector{"IDLE", "2"})); + CHECK(refusal_of("[stop]\nwhen = \"a\"\nequals = []\n").contains("equals names no value")); +} + +TEST_CASE("Scenario.RefusesAnUnknownKeyNamingIt") { + CHECK(refusal_of("second = 4\n").contains("unknown key second")); + CHECK(refusal_of("[start]\nx = 0\ny = 0\nyaw = 0\n").contains("unknown key yaw")); + CHECK(refusal_of("[[events]]\nat = 1\nsend = \"a\"\nuntil = 2\n").contains("events[0]: unknown key until")); + CHECK(refusal_of("[stop]\nwhen = \"a\"\nequals = 1\nbefore = 2\n").contains("unknown key before")); +} + +TEST_CASE("Scenario.RefusesAnEventThatIsNotExactlyOneThing") { + CHECK_FALSE(refusal_of("[[events]]\nat = 1\n").empty()); + CHECK_FALSE(refusal_of("[[events]]\nat = 1\nsend = \"a\"\npress = \"b\"\nfor = 1\n").empty()); + CHECK_FALSE(refusal_of("events = [1, 2]\n").empty()); +} + +TEST_CASE("Scenario.RefusesAnEventMissingWhatItNeeds") { + CHECK(refusal_of("[[events]]\nsend = \"a\"\n").contains("at must be a number")); + CHECK(refusal_of("[[events]]\nat = 1\npress = \"a\"\n").contains("for must be a number")); + CHECK(refusal_of("[[events]]\nat = 1\nset = \"a\"\n").contains("true or false")); + CHECK(refusal_of("[[events]]\nat = 1\n").contains("exactly one of press, set or send")); +} + +TEST_CASE("Scenario.RefusesNegativeTimes") { + CHECK(refusal_of("[[events]]\nat = -1\nsend = \"a\"\n").contains("negative")); + CHECK(refusal_of("[[events]]\nat = 1\npress = \"a\"\nfor = -0.5\n").contains("negative")); +} + +TEST_CASE("Scenario.RefusesAnIncompleteStartOrStop") { + CHECK(refusal_of("[start]\nx = 0\nyaw_deg = 0\n").contains("y must be a number")); + CHECK(refusal_of("[stop]\nequals = 1\n").contains("when must name")); + CHECK(refusal_of("[stop]\nwhen = \"a\"\nequals = true\n").contains("equals must be")); +} + +TEST_CASE("Scenario.RefusesAFileThatIsNotToml") { + CHECK(refusal_of("seconds = = 4\n").starts_with("test.toml")); + CHECK_THROWS_AS(Scenario::load("no_such_scenario.toml"), std::runtime_error); +} + +/** + * @brief A world, one input and one message: what a robot target hands a scenario. + */ +class Playing { +protected: + Playing() { + load_tiny_world(this->context.world, this->context.clock); + + this->hooks.inputs.emplace("button", &this->button); + this->hooks.messages.emplace("hello", std::vector{1, 2, 3}); + this->hooks.state_names.emplace("state", std::vector{"INIT", "IDLE", "RUN"}); + } + + /** + * @brief Play a scenario for a number of ticks. + * + * @param text The scenario. + * @param ticks Most ticks to run. + * @param variables Where the stop condition reads, or null. + * @param human Whether a human took the board over before the run. + * @param first Listener that runs before the player each tick, or null. + * @return The player, after the run. + */ + std::unique_ptr play( + std::string_view text, uint64_t ticks, const VariableSource* variables = nullptr, bool human = false, + IRunListener* first = nullptr + ) { + auto player = + std::make_unique(Scenario::parse(text), this->hooks, this->context.serial, variables); + + if (human) { + player->hand_over(); + } + + Simulation simulation(this->context, this->firmware); + + if (first != nullptr) { + simulation.add_listener(*first); + } + + simulation.add_listener(*player); + simulation.add_listener(this->probe); + simulation.run(ticks); + this->completed = simulation.completed_ticks(); + return player; + } + + // NOLINTBEGIN(*-non-private-member-variables-in-classes): the fixture is the test's own scope. + RunContext context; + bool button_level{true}; + DigitalInput button{{.name = "button", .active_low = true, .drive = [this](bool level) { + this->button_level = level; + }}}; + ScenarioHooks hooks; + Probe probe{this->button}; + uint64_t completed{0}; + + /** + * @brief Stands in for the firmware: yields once per tick and never ends. + */ + FirmwareThread firmware{[](FirmwareThread& thread) { + while (true) { + thread.yield_tick(); + } + }}; + // NOLINTEND(*-non-private-member-variables-in-classes) +}; + +TEST_CASE_FIXTURE(Playing, "Playing.PressesAnInputForTheTimeTheEventSays") { + this->play("[[events]]\nat = 0.01\npress = \"button\"\nfor = 0.02\n", 50); + + const uint64_t first = this->context.clock.tick_at(0.01); + const uint64_t last = first + this->context.clock.tick_at(0.02) - 1; + + REQUIRE_FALSE(this->probe.pressed_ticks.empty()); + CHECK_EQ(this->probe.pressed_ticks.front(), first); + CHECK_EQ(this->probe.pressed_ticks.back(), last); + CHECK_EQ(this->probe.pressed_ticks.size(), last - first + 1); + CHECK_FALSE(this->button.is_active()); + CHECK(this->button_level); +} + +TEST_CASE_FIXTURE(Playing, "Playing.DrivesTheInputsPinWithItsPolarity") { + this->play("[[events]]\nat = 0\nset = \"button\"\nvalue = true\n", 1); + + CHECK(this->button.is_active()); + CHECK_FALSE(this->button_level); +} + +TEST_CASE_FIXTURE(Playing, "Playing.LeavesTheInputsAloneOnceAHumanHasTheBoard") { + this->play("[[events]]\nat = 0.01\npress = \"button\"\nfor = 0.02\n", 50, nullptr, true); + + CHECK(this->probe.pressed_ticks.empty()); +} + +TEST_CASE_FIXTURE(Playing, "Playing.QueuesAMessageForTheFirmware") { + this->play("[[events]]\nat = 0.005\nsend = \"hello\"\n", 10); + + CHECK_EQ(this->context.serial.take_for_firmware(10), (std::vector{1, 2, 3})); +} + +TEST_CASE_FIXTURE(Playing, "Playing.RefusesAnEventOnSomethingTheRobotDoesNotHave") { + CHECK_THROWS_AS(this->play("[[events]]\nat = 0\npress = \"lever\"\nfor = 1\n", 1), std::runtime_error); + CHECK_THROWS_AS(this->play("[[events]]\nat = 0\nsend = \"goodbye\"\n", 1), std::runtime_error); +} + +TEST_CASE_FIXTURE(Playing, "Playing.RefusesAStopOnAStateTheVariableDoesNotHave") { + CHECK_THROWS_AS(this->play("[stop]\nwhen = \"state\"\nequals = \"LOST\"\n", 1), std::runtime_error); + CHECK_THROWS_AS(this->play("[stop]\nwhen = \"lap\"\nequals = \"LAST\"\n", 1), std::runtime_error); +} + +TEST_CASE_FIXTURE(Playing, "Playing.StopsTheRunOnceTheConditionHoldsAfterItsDelay") { + const MapVariables variables({{"state", 1.0}}); + const auto player = this->play("[stop]\nwhen = \"state\"\nequals = \"IDLE\"\nafter = 0.1\n", 1000, &variables); + + const auto stop_tick = static_cast(std::ceil(0.1 / 1042e-6)); + + REQUIRE(player->stopped_at().has_value()); + CHECK_EQ(this->completed, stop_tick); + CHECK_EQ( + player->stopped_at().value_or(-1.0), + doctest::Approx(static_cast(stop_tick) * 1042 * 1e-6).epsilon(1e-12) + ); + CHECK_GE(player->stopped_at().value_or(-1.0), 0.1); +} + +TEST_CASE_FIXTURE(Playing, "Playing.StopsOnANumericValue") { + const MapVariables variables({{"lap", 3.0}}); + const auto player = this->play("[stop]\nwhen = \"lap\"\nequals = 3\n", 100, &variables); + + CHECK_EQ(player->stopped_at(), 0.0); + CHECK_EQ(this->completed, 0U); +} + +TEST_CASE_FIXTURE(Playing, "Playing.StopsOnAnyOfSeveralValues") { + const MapVariables variables({{"state", 2.0}}); + const auto player = this->play("[stop]\nwhen = \"state\"\nequals = [\"IDLE\", \"RUN\"]\n", 100, &variables); + + CHECK_EQ(player->stopped_at(), 0.0); + CHECK_EQ(this->completed, 0U); +} + +TEST_CASE_FIXTURE(Playing, "Playing.StopsOnlyOnceTheValueIsReachedTheCountedTimes") { + SteppedVariables variables("state", {{0, 1.0}, {10, 2.0}, {20, 1.0}, {30, 2.0}, {40, 1.0}}); + const auto player = + this->play("[stop]\nwhen = \"state\"\nequals = \"IDLE\"\ncount = 3\n", 100, &variables, false, &variables); + + CHECK_EQ(this->completed, 40U); + CHECK(player->stopped_at().has_value()); +} + +TEST_CASE_FIXTURE(Playing, "Playing.AbortsOnTheFirstAbortValueWhateverTheCount") { + SteppedVariables variables("state", {{0, 1.0}, {10, 0.0}}); + const auto player = this->play( + "[stop]\nwhen = \"state\"\nequals = \"IDLE\"\ncount = 3\nabort = [\"INIT\"]\n", 100, &variables, false, + &variables + ); + + CHECK_EQ(this->completed, 10U); + CHECK(player->stopped_at().has_value()); +} + +TEST_CASE_FIXTURE(Playing, "Playing.HoldsAnEventUntilItsConditionHolds") { + SteppedVariables variables("state", {{0, 1.0}, {25, 2.0}}); + this->play( + "[[events]]\nat = 0\npress = \"button\"\nfor = 0.002\nwhen = \"state\"\nequals = \"RUN\"\n", 40, &variables, + false, &variables + ); + + REQUIRE_FALSE(this->probe.pressed_ticks.empty()); + CHECK_EQ(this->probe.pressed_ticks.front(), 25U); +} + +TEST_CASE("Scenario.ReadsAnEventConditionAndAStopCount") { + const Scenario scenario = Scenario::parse( + "[[events]]\nat = 1\nsend = \"a\"\nwhen = \"state\"\nequals = \"IDLE\"\n" + "[stop]\nwhen = \"state\"\nequals = \"IDLE\"\ncount = 2\n" + ); + + REQUIRE_EQ(scenario.events.size(), 1U); + CHECK_EQ(scenario.events.front().when, "state"); + CHECK_EQ(scenario.events.front().equals, "IDLE"); + CHECK_EQ(scenario.stop.value_or(StopCondition{}).count, 2); + CHECK(refusal_of("[[events]]\nat = 1\nsend = \"a\"\nwhen = \"state\"\n").contains("needs both")); + CHECK(refusal_of("[stop]\nwhen = \"a\"\nequals = 1\ncount = 0\n").contains("count must be")); +} + +TEST_CASE_FIXTURE(Playing, "Playing.RunsToTheEndWhileTheConditionDoesNotHold") { + const MapVariables variables({{"state", 2.0}}); + const auto player = this->play("[stop]\nwhen = \"state\"\nequals = \"IDLE\"\n", 20, &variables); + + CHECK_FALSE(player->stopped_at().has_value()); + CHECK_EQ(this->completed, 20U); +} + +TEST_CASE_FIXTURE(Playing, "Playing.NeverStopsWithoutAnythingToReadTheVariableFrom") { + const auto player = this->play("[stop]\nwhen = \"state\"\nequals = \"INIT\"\n", 20); + + CHECK_FALSE(player->stopped_at().has_value()); + CHECK_EQ(this->completed, 20U); } } // namespace } // namespace micras::sim diff --git a/tests/unit/serial_bus_test.cpp b/tests/unit/serial_bus_test.cpp index 71041fd..d9b7388 100644 --- a/tests/unit/serial_bus_test.cpp +++ b/tests/unit/serial_bus_test.cpp @@ -1,70 +1,62 @@ -#include +#include +#include + +#include #include "micras/sim/core/serial_bus.hpp" +#include "support.hpp" namespace micras::sim { namespace { -/** - * @brief Listener that keeps every byte it was handed. - */ -class RecordingListener : public ISerialListener { -public: - void on_firmware_bytes(std::span bytes) override { - this->received.insert(this->received.end(), bytes.begin(), bytes.end()); - } - - std::vector received; -}; - -TEST(SerialBus, FansFirmwareOutputOutToEveryListener) { - SerialBus bus; - RecordingListener first; - RecordingListener second; +TEST_CASE("SerialBus.FansFirmwareOutputOutToEveryListener") { + SerialBus bus; + ByteCollector first; + ByteCollector second; bus.add_listener(first); bus.add_listener(second); bus.send_from_firmware(std::vector{1, 2}); bus.send_from_firmware(std::vector{3}); - EXPECT_EQ(first.received, (std::vector{1, 2, 3})); - EXPECT_EQ(second.received, first.received); + CHECK_EQ(first.received, (std::vector{1, 2, 3})); + CHECK_EQ(second.received, first.received); } -TEST(SerialBus, StopsDeliveringToARemovedListener) { - SerialBus bus; - RecordingListener listener; +TEST_CASE("SerialBus.StopsDeliveringToARemovedListener") { + SerialBus bus; + ByteCollector listener; bus.add_listener(listener); bus.send_from_firmware(std::vector{1}); bus.remove_listener(listener); bus.send_from_firmware(std::vector{2}); - EXPECT_EQ(listener.received, (std::vector{1})); + CHECK_EQ(listener.received, (std::vector{1})); } -TEST(SerialBus, DropsFirmwareOutputWhenNobodyIsListening) { +TEST_CASE("SerialBus.DropsFirmwareOutputWhenNobodyIsListening") { SerialBus bus; - EXPECT_NO_THROW(bus.send_from_firmware(std::vector{1})); + CHECK_NOTHROW(bus.send_from_firmware(std::vector{1})); } -TEST(SerialBus, HandsTheFirmwareOnePacketAtATime) { +TEST_CASE("SerialBus.HandsTheFirmwareTheBytesInTheOrderTheyWereQueued") { SerialBus bus; bus.queue_for_firmware(std::vector{1, 2}); bus.queue_for_firmware(std::vector{3}); - EXPECT_EQ(bus.pending_packets(), 2U); - EXPECT_EQ(bus.take_for_firmware(), (std::vector{1, 2})); - EXPECT_EQ(bus.take_for_firmware(), (std::vector{3})); - EXPECT_TRUE(bus.take_for_firmware().empty()); - EXPECT_EQ(bus.pending_packets(), 0U); + CHECK_EQ(bus.take_for_firmware(2), (std::vector{1, 2})); + CHECK_EQ(bus.take_for_firmware(5), (std::vector{3})); + CHECK(bus.take_for_firmware(1).empty()); } -TEST(SerialBus, ClearDropsEveryQueuedPacket) { - SerialBus bus; - bus.queue_for_firmware(std::vector{1}); - bus.clear(); +TEST_CASE("SerialBus.DropsAndCountsBytesPastTheBound") { + SerialBus bus; + const std::vector bytes(SerialBus::max_pending_bytes + 3, 7); + + bus.queue_for_firmware(bytes); - EXPECT_TRUE(bus.take_for_firmware().empty()); + CHECK_EQ(bus.dropped_bytes(), 3U); + CHECK_EQ(bus.take_for_firmware(SerialBus::max_pending_bytes * 2).size(), SerialBus::max_pending_bytes); } } // namespace } // namespace micras::sim diff --git a/tests/unit/shadow_conformance_test.cpp b/tests/unit/shadow_conformance_test.cpp deleted file mode 100644 index f1c258e..0000000 --- a/tests/unit/shadow_conformance_test.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/** - * @file - * - * @brief Compile-time conformance of the shadow proxy configs with the firmware ones. - * - * @note Every Config below is initialised with the designators the firmware - * target.hpp uses for the fields both sides share. Renaming a field in - * the firmware (start_page -> start_sector, say) breaks this translation - * unit instead of silently drifting. Hardware-only fields (pwm, gpio, - * spi, adc) have no shadow counterpart and are not listed. - */ - -#include - -#include "target.hpp" - -namespace micras { -namespace { -const proxy::Storage::Config storage_shape{.start_page = 0, .number_of_pages = 0}; - -const proxy::Button::Config button_shape{.long_press_delay = 0, .extra_long_press_delay = 0}; - -const proxy::Fan::Config fan_shape{.max_acceleration = 0.0F}; - -const proxy::Locomotion::Config locomotion_shape{.left_motor = {}, .right_motor = {}}; - -const proxy::TorqueSensors::Config torque_sensors_shape{.shunt_resistor = 0.0F, .max_torque = 0.0F}; - -const proxy::WallSensors::Config wall_sensors_shape{ - .uncertainty = 0.0F, .base_readings = {}, .filter_cutoff = 0.0F, .sampling_frequency = 0.0F -}; - -TEST(ShadowConformance, SharedConfigFieldsCompile) { - EXPECT_EQ(storage_shape.number_of_pages, 0); - EXPECT_EQ(button_shape.long_press_delay, 0); - EXPECT_FLOAT_EQ(fan_shape.max_acceleration, 0.0F); - EXPECT_TRUE(locomotion_shape.left_motor.actuator.empty()); - EXPECT_FLOAT_EQ(torque_sensors_shape.max_torque, 0.0F); - EXPECT_FLOAT_EQ(wall_sensors_shape.filter_cutoff, 0.0F); -} - -TEST(ShadowConformance, TargetConfigsUseFirmwareDelays) { - EXPECT_EQ(button_config.long_press_delay, 500U); - EXPECT_EQ(button_config.extra_long_press_delay, 2000U); - EXPECT_FLOAT_EQ(wall_sensors_config.sampling_frequency, loop_frequency); -} -} // namespace -} // namespace micras diff --git a/tests/unit/simulation_test.cpp b/tests/unit/simulation_test.cpp index 5712b4a..80e44e2 100644 --- a/tests/unit/simulation_test.cpp +++ b/tests/unit/simulation_test.cpp @@ -1,9 +1,14 @@ +#include #include +#include #include -#include +#include +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" #include "micras/sim/core/simulation.hpp" +#include "support.hpp" namespace micras::sim { namespace { @@ -35,40 +40,36 @@ class TracingListener : public IRunListener { }; /** - * @brief A world and a clock configured like every recorded run. + * @brief The tiny robot's world and a clock of two steps per tick. */ -class Run : public testing::Test { +class Run { protected: - void SetUp() override { - this->context.world.load(MICRAS_TEST_MODEL); - this->context.clock = Clock::from_model(this->context.world.timestep(), 1042); - this->context.world.reset(); - } + Run() { load_tiny_world(this->context.world, this->context.clock); } // NOLINTNEXTLINE(*-non-private-member-variables-in-classes): the fixture is the test's own scope. - SimulationContext context; + RunContext context; /** * @brief Stands in for the firmware: yields once per tick and never ends. */ // NOLINTNEXTLINE(*-non-private-member-variables-in-classes): the fixture is the test's own scope. - FirmwareThread firmware{[this] { + FirmwareThread firmware{[](FirmwareThread& thread) { while (true) { - this->firmware.yield_tick(); + thread.yield_tick(); } }}; }; -TEST_F(Run, AdvancesTheClockOncePerTick) { +TEST_CASE_FIXTURE(Run, "Run.AdvancesTheClockOncePerTick") { Simulation simulation(this->context, this->firmware); simulation.run(5); - EXPECT_EQ(this->context.clock.tick_count(), 5U); - EXPECT_EQ(this->context.clock.now_us(), 5U * 1042U); - EXPECT_EQ(simulation.completed_ticks(), 5U); + CHECK_EQ(this->context.clock.tick_count(), 5U); + CHECK_EQ(this->context.clock.now_us(), 5U * 1042U); + CHECK_EQ(simulation.completed_ticks(), 5U); } -TEST_F(Run, CallsListenersInRegistrationOrder) { +TEST_CASE_FIXTURE(Run, "Run.CallsListenersInRegistrationOrder") { std::vector trace; TracingListener first(trace, "a"); TracingListener second(trace, "b"); @@ -80,10 +81,10 @@ TEST_F(Run, CallsListenersInRegistrationOrder) { const std::vector expected{"a:start", "b:start", "a:before0", "b:before0", "a:after0", "b:after0", "a:finish", "b:finish"}; - EXPECT_EQ(trace, expected); + CHECK_EQ(trace, expected); } -TEST_F(Run, AListenerCanStopTheRunBeforeTheTick) { +TEST_CASE_FIXTURE(Run, "Run.AListenerCanStopTheRunBeforeTheTick") { std::vector trace; TracingListener quitter(trace, "q", RunControl::QUIT); @@ -91,34 +92,32 @@ TEST_F(Run, AListenerCanStopTheRunBeforeTheTick) { simulation.add_listener(quitter); simulation.run(10); - EXPECT_EQ(simulation.completed_ticks(), 0U); - EXPECT_EQ(this->context.clock.tick_count(), 0U); - EXPECT_EQ(trace, (std::vector{"q:start", "q:before0", "q:finish"})); + CHECK_EQ(simulation.completed_ticks(), 0U); + CHECK_EQ(this->context.clock.tick_count(), 0U); + CHECK_EQ(trace, (std::vector{"q:start", "q:before0", "q:finish"})); } -TEST_F(Run, StopsWhenTheProgramEnds) { - int ticks = 0; - FirmwareThread* handle = nullptr; +TEST_CASE_FIXTURE(Run, "Run.StopsWhenTheProgramEnds") { + int ticks = 0; - FirmwareThread program([&] { + FirmwareThread program([&](FirmwareThread& thread) { for (ticks = 0; ticks < 3; ticks++) { - handle->yield_tick(); + thread.yield_tick(); } }); - handle = &program; Simulation simulation(this->context, program); simulation.run(100); - EXPECT_LT(simulation.completed_ticks(), 100U); - EXPECT_EQ(ticks, 3); + CHECK_LT(simulation.completed_ticks(), 100U); + CHECK_EQ(ticks, 3); } -TEST_F(Run, CarriesAProgramErrorOutOfRun) { - FirmwareThread program([] { throw std::runtime_error("the firmware gave up"); }); +TEST_CASE_FIXTURE(Run, "Run.CarriesAProgramErrorOutOfRun") { + FirmwareThread program([](FirmwareThread&) { throw std::runtime_error("the firmware gave up"); }); Simulation simulation(this->context, program); - EXPECT_THROW(simulation.run(5), std::runtime_error); + CHECK_THROWS_AS(simulation.run(5), std::runtime_error); } } // namespace } // namespace micras::sim diff --git a/tests/unit/support.hpp b/tests/unit/support.hpp new file mode 100644 index 0000000..f17f065 --- /dev/null +++ b/tests/unit/support.hpp @@ -0,0 +1,94 @@ +/** + * @file + * + * @brief What several of the engine's tests share: the tiny robot's world, bytes collected from the + * firmware, and variables set by hand. + */ + +#ifndef MICRAS_SIM_TESTS_SUPPORT_HPP +#define MICRAS_SIM_TESTS_SUPPORT_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "micras/sim/core/clock.hpp" +#include "micras/sim/core/mujoco_world.hpp" +#include "micras/sim/core/serial_bus.hpp" +#include "micras/sim/core/variable_source.hpp" + +namespace micras::sim { +/** + * @brief Load the tiny robot, with a clock of two steps per 1042 us tick, and reset it. + * + * @param world World to load it into. + * @param clock Clock to configure. + */ +inline void load_tiny_world(MujocoWorld& world, Clock& clock) { + world.load(MICRAS_TEST_MODEL); + clock = Clock::from_model(world.timestep(), 1042); + world.reset(); +} + +/** + * @brief Listener that keeps every byte the firmware sent. + */ +class ByteCollector : public ISerialListener { +public: + /** + * @brief Keep the bytes. + * + * @param bytes Bytes leaving the firmware. + */ + void on_firmware_bytes(std::span bytes) override { + this->received.insert(this->received.end(), bytes.begin(), bytes.end()); + } + + // NOLINTNEXTLINE(*-non-private-member-variables-in-classes): the test reads what arrived. + std::vector received; +}; + +/** + * @brief Variables the test sets, NaN for any name it has not set. + */ +class MapVariables : public VariableSource { +public: + /** + * @brief Start with some values. + * + * @param values Values by name. + */ + explicit MapVariables(std::map values = {}) : values{std::move(values)} { } + + /** + * @brief Get a variable. + * + * @param name Name of the variable. + * @return Its value, or NaN when it was never set. + */ + double value_of(const std::string& name) const override { + const auto found = this->values.find(name); + return found == this->values.end() ? std::nan("") : found->second; + } + + /** + * @brief Set a variable. + * + * @param name Name of the variable. + * @param value Its new value. + */ + void set(const std::string& name, double value) { this->values[name] = value; } + +private: + /** + * @brief Values by name. + */ + std::map values; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_TESTS_SUPPORT_HPP diff --git a/tests/unit/telemetry_test.cpp b/tests/unit/telemetry_test.cpp deleted file mode 100644 index 07a4351..0000000 --- a/tests/unit/telemetry_test.cpp +++ /dev/null @@ -1,171 +0,0 @@ -#include -#include - -#include - -#include "micras/comm/packet.hpp" -#include "micras/sim/telemetry/telemetry.hpp" - -namespace micras::sim { -namespace { -void append_string(std::vector& payload, const std::string& text) { - payload.push_back(static_cast(text.size())); - payload.insert(payload.end(), text.begin(), text.end()); -} - -void append_variable( - std::vector& payload, uint16_t id, const std::string& name, const std::string& type, bool read_only -) { - payload.push_back(static_cast(id & 0xFF)); - payload.push_back(static_cast(id >> 8)); - append_string(payload, name); - append_string(payload, type); - payload.push_back(read_only ? 1 : 0); -} - -std::vector map_payload() { - std::vector payload{3, 0}; - append_variable(payload, 7, "Grid Pose", "micras::nav::GridPose", true); - append_variable(payload, 2, "Explore", "bool", false); - append_variable(payload, 5, "Linear Speed", "float", true); - return payload; -} - -std::vector float_bytes(float value) { - std::vector bytes(sizeof(float)); - std::memcpy(bytes.data(), &value, sizeof(float)); - return bytes; -} - -TEST(VariableMap, OrdersMonitoringVariablesById) { - const VariableMap map = VariableMap::parse(map_payload()).value_or(VariableMap{}); - - const auto& monitoring = map.monitoring_variables(); - ASSERT_EQ(monitoring.size(), 2U); - EXPECT_EQ(monitoring[0].id, 5); - EXPECT_EQ(monitoring[0].name, "Linear Speed"); - EXPECT_EQ(monitoring[1].id, 7); - EXPECT_EQ(monitoring[1].type, "micras::nav::GridPose"); -} - -TEST(VariableMap, ResolvesNamesAndTypes) { - const VariableMap map = VariableMap::parse(map_payload()).value_or(VariableMap{}); - - EXPECT_EQ(map.find_id("Explore"), std::optional{2}); - EXPECT_EQ(map.find_id("Missing"), std::nullopt); - EXPECT_EQ(map.type_of(7), "micras::nav::GridPose"); - EXPECT_EQ(map.type_of(99), ""); -} - -TEST(VariableMap, RejectsTruncatedPayload) { - auto payload = map_payload(); - payload.pop_back(); - EXPECT_FALSE(VariableMap::parse(payload).has_value()); - EXPECT_FALSE(VariableMap::parse({1}).has_value()); -} - -TEST(VariableMap, ExpandsCustomSerializables) { - EXPECT_EQ(VariableMap::type_column_suffixes("micras::nav::GridPose"), (std::vector{"x", "y", "side"})); - EXPECT_EQ(VariableMap::type_column_suffixes("micras::nav::State").size(), 5U); - EXPECT_TRUE(VariableMap::type_column_suffixes("float").empty()); -} - -TEST(PoolValues, DecodesScalars) { - EXPECT_FLOAT_EQ(static_cast(PoolValues::decode("float", float_bytes(1.5F))[0]), 1.5F); - EXPECT_EQ(PoolValues::decode("bool", {1}), (std::vector{1.0})); - EXPECT_EQ(PoolValues::decode("unsigned char", {200}), (std::vector{200.0})); -} - -TEST(PoolValues, DecodesGridPoseAndState) { - EXPECT_EQ(PoolValues::decode("micras::nav::GridPose", {3, 4, 2}), (std::vector{3.0, 4.0, 2.0})); - - std::vector state; - - for (const float value : {1.0F, 2.0F, 3.0F, 4.0F, 5.0F}) { - const auto bytes = float_bytes(value); - state.insert(state.end(), bytes.begin(), bytes.end()); - } - - EXPECT_EQ(PoolValues::decode("micras::nav::State", state), (std::vector{1.0, 2.0, 3.0, 4.0, 5.0})); -} - -TEST(PoolValues, UnknownTypesDecodeToNan) { - const auto decoded = PoolValues::decode("uint32_t", {1, 2, 3, 4}); - ASSERT_EQ(decoded.size(), 1U); - EXPECT_TRUE(std::isnan(decoded[0])); - - const auto mismatched = PoolValues::decode("micras::nav::GridPose", {1}); - ASSERT_EQ(mismatched.size(), 3U); - EXPECT_TRUE(std::isnan(mismatched[2])); -} - -TEST(PoolValues, StoresLatestValue) { - PoolValues values; - EXPECT_TRUE(std::isnan(values.get(5))); - - values.store(5, "float", float_bytes(2.0F)); - values.store(5, "float", float_bytes(3.0F)); - EXPECT_DOUBLE_EQ(values.get(5), 3.0); - EXPECT_TRUE(std::isnan(values.get(5, 1))); -} - -std::vector map_response() { - return comm::Packet(comm::Packet::MessageType::SERIAL_VARIABLE_MAP_RESPONSE, map_payload()).serialize(); -} - -TEST(Telemetry, DecodesTheMapAndTheValuesTheFirmwareSends) { - SerialBus bus; - Telemetry telemetry(bus); - - EXPECT_FALSE(telemetry.has_variable_map()); - - bus.send_from_firmware(map_response()); - telemetry.poll(); - - ASSERT_TRUE(telemetry.has_variable_map()); - EXPECT_EQ(telemetry.variable_map().monitoring_variables().size(), 2U); - EXPECT_TRUE(std::isnan(telemetry.value_of("Linear Speed"))); - - bus.send_from_firmware(comm::Packet(comm::Packet::MessageType::SERIAL_VARIABLE, 5, float_bytes(1.25F)).serialize()); - telemetry.poll(); - - EXPECT_DOUBLE_EQ(telemetry.value_of("Linear Speed"), 1.25); - EXPECT_TRUE(std::isnan(telemetry.value_of("Missing"))); - EXPECT_EQ(telemetry.resync_count(), 0U); - EXPECT_EQ(telemetry.map_failure_count(), 0U); -} - -TEST(Telemetry, QueuesRequestsForTheFirmware) { - SerialBus bus; - Telemetry telemetry(bus); - - telemetry.request_variable_map(); - const std::vector request = bus.take_for_firmware(); - ASSERT_FALSE(request.empty()); - EXPECT_EQ(comm::Packet(request).get_type(), comm::Packet::MessageType::SERIAL_VARIABLE_MAP_REQUEST); - - telemetry.send_variable(2, {0x01}); - const std::vector command = bus.take_for_firmware(); - ASSERT_FALSE(command.empty()); - - const comm::Packet packet(command); - EXPECT_EQ(packet.get_type(), comm::Packet::MessageType::SERIAL_VARIABLE); - EXPECT_EQ(packet.get_id(), 2); - EXPECT_EQ(packet.get_payload(), (std::vector{0x01})); -} - -TEST(Telemetry, CountsMalformedMapResponsesInsteadOfAcceptingThem) { - SerialBus bus; - Telemetry telemetry(bus); - - std::vector truncated = map_payload(); - truncated.pop_back(); - bus.send_from_firmware(comm::Packet(comm::Packet::MessageType::SERIAL_VARIABLE_MAP_RESPONSE, truncated).serialize() - ); - telemetry.poll(); - - EXPECT_FALSE(telemetry.has_variable_map()); - EXPECT_EQ(telemetry.map_failure_count(), 1U); -} -} // namespace -} // namespace micras::sim diff --git a/tests/unit/temp_file.hpp b/tests/unit/temp_file.hpp new file mode 100644 index 0000000..6ea3af2 --- /dev/null +++ b/tests/unit/temp_file.hpp @@ -0,0 +1,84 @@ +/** + * @file + * + * @brief A file in the system's temporary directory that belongs to one test process. + */ + +#ifndef MICRAS_SIM_TESTS_TEMP_FILE_HPP +#define MICRAS_SIM_TESTS_TEMP_FILE_HPP + +#include +#include +#include +#include + +#include + +namespace micras::sim { +/** + * @brief Path in the temporary directory named after the process, removed when it goes out of scope. + * + * @details ctest runs every test case in its own process, several at once, so the process id keeps two cases + * that use the same name from writing the same file. + */ +class TempFile { +public: + /** + * @brief Name the file, without creating it. + * + * @param name Name of the file, unique within the process. + */ + explicit TempFile(std::string_view name) : + file_path{ + std::filesystem::temp_directory_path() / + ("micras_sim_" + std::to_string(::getpid()) + "_" + std::string(name)) + } { } + + /** + * @brief Remove the file, if it exists. + */ + ~TempFile() { + std::error_code error; + std::filesystem::remove(this->file_path, error); + } + + /** + * @brief Copying would remove the file twice. + */ + TempFile(const TempFile&) = delete; + + /** + * @brief Moving would remove the file twice. + */ + TempFile(TempFile&&) = delete; + + /** + * @brief Copying would remove the file twice. + * + * @return Never returns. + */ + TempFile& operator=(const TempFile&) = delete; + + /** + * @brief Moving would remove the file twice. + * + * @return Never returns. + */ + TempFile& operator=(TempFile&&) = delete; + + /** + * @brief Get the path of the file. + * + * @return The path. + */ + const std::filesystem::path& path() const { return this->file_path; } + +private: + /** + * @brief Path of the file. + */ + std::filesystem::path file_path; +}; +} // namespace micras::sim + +#endif // MICRAS_SIM_TESTS_TEMP_FILE_HPP diff --git a/tests/unit/web_socket_server_test.cpp b/tests/unit/web_socket_server_test.cpp new file mode 100644 index 0000000..ee9193f --- /dev/null +++ b/tests/unit/web_socket_server_test.cpp @@ -0,0 +1,280 @@ +/** + * @file + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "micras/sim/bridge/monitor_bridge.hpp" +#include "micras/sim/bridge/web_socket_server.hpp" +#include "micras/sim/core/firmware_thread.hpp" +#include "micras/sim/core/run_context.hpp" +#include "micras/sim/core/simulation.hpp" +#include "support.hpp" + +namespace micras::sim { +namespace { +using std::chrono::milliseconds; +using std::chrono::seconds; +using std::chrono::steady_clock; + +/** + * @brief Owns a POSIX socket and closes it. + */ +class Socket { +public: + Socket() : descriptor{socket(AF_INET, SOCK_STREAM, 0)} { } + + Socket(const Socket&) = delete; + Socket(Socket&&) = delete; + Socket& operator=(const Socket&) = delete; + Socket& operator=(Socket&&) = delete; + + ~Socket() { close(this->descriptor); } + + int get() const { return this->descriptor; } + +private: + int descriptor; +}; +} // namespace + +/** + * @brief Build a loopback address. + * + * @param port Port in host order, 0 for any. + * @return The address. + */ +static sockaddr_in loopback(uint16_t port) { + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(port); + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + return address; +} + +/** + * @brief Bind a socket to a port the system picks, listening. + * + * @param socket Socket to bind. + * @return The port it holds. + */ +static uint16_t hold_free_port(const Socket& socket) { + sockaddr_in address = loopback(0); + socklen_t length = sizeof(address); + + REQUIRE_GE(socket.get(), 0); + + // NOLINTBEGIN(*-reinterpret-cast): the sockets API takes its addresses as sockaddr. + CHECK_EQ(bind(socket.get(), reinterpret_cast(&address), sizeof(address)), 0); + CHECK_EQ(listen(socket.get(), 1), 0); + CHECK_EQ(getsockname(socket.get(), reinterpret_cast(&address), &length), 0); + // NOLINTEND(*-reinterpret-cast) + + return ntohs(address.sin_port); +} + +/** + * @brief Find a port nobody listens on. + * + * @return The port, released again. + */ +static uint16_t free_port() { + const Socket socket; + return hold_free_port(socket); +} + +static uint16_t start_on_a_free_port(const std::function& start) { + constexpr int attempts{20}; + + for (int attempt = 0; attempt < attempts; attempt++) { + const uint16_t port = free_port(); + + if (start(port)) { + return port; + } + } + + return 0; +} + +/** + * @brief Open a WebSocket connection, then never read from it again. + * + * @note The receive buffer is made as small as the system allows, so the + * server's writes back up after a few frames. + * + * @param socket Socket to connect. + * @param port Port the server listens on. + * @return True once the server answered the handshake. + */ +static bool connect_and_stall(const Socket& socket, uint16_t port) { + const int receive_buffer = 1024; + // NOLINTNEXTLINE(misc-include-cleaner): provides them through a kernel header. + setsockopt(socket.get(), SOL_SOCKET, SO_RCVBUF, &receive_buffer, sizeof(receive_buffer)); + + sockaddr_in address = loopback(port); + + // NOLINTNEXTLINE(*-reinterpret-cast): the sockets API takes its addresses as sockaddr. + if (connect(socket.get(), reinterpret_cast(&address), sizeof(address)) != 0) { + return false; + } + + const std::string request = "GET / HTTP/1.1\r\nHost: localhost\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n" + "Sec-WebSocket-Key: dGhlIHNhbXBsZSBub25jZQ==\r\nSec-WebSocket-Version: 13\r\n\r\n"; + + if (send(socket.get(), request.data(), request.size(), 0) != static_cast(request.size())) { + return false; + } + + std::string response; + std::array character{}; + + while (!response.contains("\r\n\r\n")) { + if (recv(socket.get(), character.data(), 1, 0) != 1) { + return false; + } + + response.push_back(character.at(0)); + } + + return response.starts_with("HTTP/1.1 101"); +} + +/** + * @brief Send one binary frame from the client side, masked as the protocol requires. + * + * @param socket Connected socket. + * @param payload Bytes to send, fewer than 126. + * @return True when the whole frame was written. + */ +static bool send_binary(const Socket& socket, const std::vector& payload) { + const std::array mask{0x12, 0x34, 0x56, 0x78}; + std::vector frame{0x82, static_cast(0x80 | payload.size())}; + frame.insert(frame.end(), mask.begin(), mask.end()); + + for (std::size_t index = 0; index < payload.size(); index++) { + frame.push_back(payload.at(index) ^ mask.at(index % mask.size())); + } + + return send(socket.get(), frame.data(), frame.size(), 0) == static_cast(frame.size()); +} + +/** + * @brief Read one unmasked binary frame the server sent. + * + * @param socket Connected socket. + * @return The frame's payload, empty when it was not a short binary frame. + */ +static std::vector receive_binary(const Socket& socket) { + std::array head{}; + + if (recv(socket.get(), head.data(), head.size(), MSG_WAITALL) != 2 or head.at(0) != 0x82 or head.at(1) >= 126) { + return {}; + } + + std::vector payload(head.at(1)); + + if (recv(socket.get(), payload.data(), payload.size(), MSG_WAITALL) != static_cast(payload.size())) { + return {}; + } + + return payload; +} + +namespace { +TEST_CASE("MonitorBridge.CarriesRawBytesBothWays") { + RunContext context; + load_tiny_world(context.world, context.clock); + FirmwareThread firmware{[](FirmwareThread& thread) { + while (thread.yield_tick()) { } + }}; + const Simulation simulation(context, firmware); + + std::vector inbound; + std::string error; + std::unique_ptr bridge; + const uint16_t port = start_on_a_free_port([&](uint16_t candidate) { + bridge = std::make_unique(context.serial, candidate, error); + return bridge->is_open(); + }); + REQUIRE_MESSAGE(port != 0, error); + + const Socket client; + REQUIRE(connect_and_stall(client, port)); + + const std::vector command{0x00, 0x01, 0x7E, 0xFF, 0x00}; + REQUIRE(send_binary(client, command)); + + const auto deadline = steady_clock::now() + seconds(10); + + while (inbound.empty() and steady_clock::now() < deadline) { + bridge->on_before_tick(simulation); + inbound = context.serial.take_for_firmware(command.size()); + std::this_thread::sleep_for(milliseconds(1)); + } + + CHECK_EQ(inbound, command); + CHECK(bridge->was_interactive()); + + const std::vector telemetry{0x02, 0x00, 0xAA, 0x00}; + context.serial.send_from_firmware(telemetry); + bridge->on_after_tick(simulation); + + CHECK_EQ(receive_binary(client), telemetry); +} + +TEST_CASE("WebSocketServer.AClientThatStopsReadingNeverStallsTheCaller") { + WebSocketServer server; + std::string error; + const uint16_t port = start_on_a_free_port([&](uint16_t candidate) { return server.start(candidate, error); }); + REQUIRE_MESSAGE(port != 0, error); + + const Socket client; + REQUIRE(connect_and_stall(client, port)); + + const std::vector frame(std::size_t{64} * 1024, 0xAB); + const auto deadline = steady_clock::now() + seconds(10); + auto slowest = steady_clock::duration::zero(); + + while (server.dropped_frames() == 0 and steady_clock::now() < deadline) { + const auto start = steady_clock::now(); + server.broadcast(frame); + slowest = std::max(slowest, steady_clock::now() - start); + } + + CHECK_GT(server.dropped_frames(), 0U); + CHECK_LT(slowest, milliseconds(100)); + + auto stopping = std::async(std::launch::async, [&server] { server.stop(); }); + CHECK_EQ(stopping.wait_for(seconds(10)), std::future_status::ready); +} + +TEST_CASE("WebSocketServer.RefusesAPortSomebodyElseHolds") { + const Socket holder; + const uint16_t port = hold_free_port(holder); + + WebSocketServer server; + std::string error; + + CHECK_FALSE(server.start(port, error)); + CHECK_FALSE(error.empty()); +} +} // namespace +} // namespace micras::sim diff --git a/third_party/README.md b/third_party/README.md deleted file mode 100644 index d3a3773..0000000 --- a/third_party/README.md +++ /dev/null @@ -1 +0,0 @@ -# Fetched dependencies land in _deps/ (gitignored); see cmake/third_party.cmake for the pins. diff --git a/tools/analyze.py b/tools/analyze.py index 9a812a6..6362607 100755 --- a/tools/analyze.py +++ b/tools/analyze.py @@ -1,34 +1,43 @@ #!/usr/bin/env python3 -"""Analyse one micras_simulation run directory. - -Reads ``/data.csv`` (and ``/meta.json`` when present) and writes -``report.json`` plus a set of PNGs next to them. - -Conventions taken from the harness: - -* The model's forward axis is **+y in body frame** (``models/robot_legacy.xml`` - puts the lidars and the caster at positive y). -* ``yaw`` is produced by ``src/sim/recorder.cpp::to_euler`` as - ``atan2(2(wz + xy), 1 - 2(y^2 + z^2))``, i.e. a ZYX yaw about the world +z - axis, zero when the body +y axis points along the world +y axis. - The body forward direction in world coordinates is therefore - ``(-sin(yaw), cos(yaw))`` and the ground-truth forward speed is - ``-vx_world*sin(yaw) + vy_world*cos(yaw)``. Current harnesses write that - projection out as the ``v_forward`` column and this script prefers it; - older runs whose CSV only has ``vx``/``vy`` are still handled. -* ``*_penetration`` columns use NaN as the "geom had no contact this tick" - sentinel, so they are excluded from the non-finite sample count. -* Wheel radius is 0.011 m (``models/robot_legacy.xml``, ``class="wheel"``). - -Only numpy and matplotlib are used; pandas is not available on this machine. +"""Analyze one simulation run directory. + +Reads ``/data.csv`` and ``/meta.json`` and writes ``report.json`` plus a +set of PNGs next to them. + +This script knows the engine's columns only: the body block every row starts with +(``x``, ``y``, ``z``, ``roll``, ``pitch``, ``yaw``, ``v_forward``, ``wz_body``) and +the kinds of column a robot target configures, recognized by their suffix or +prefix (``