From 6078f1f160635f87cf1d5335d024440c6064a70f Mon Sep 17 00:00:00 2001 From: jpptm Date: Sun, 13 Sep 2026 11:59:59 +1000 Subject: [PATCH 1/2] Import the NUbots formatter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds `./b format`, a port of NUbots' tools/format.py, plus the .clang-format and .cmake-format.py configs it drives. Formatter versions are pinned in pyproject.toml to the ones NUbots uses (clang-format 14.0.6 — the same LLVM release NUbots builds into its image — cmakelang 0.6.13, isort 5.13.2, black 24.10.0), so a given file formats identically in either repo. The port differs from the original only where NUSim's plumbing forces it: * the formatters run on the host out of the uv project environment rather than inside the docker image, since ./b here is a stdlib-only host-side dispatcher * --check diffs with difflib instead of shelling out to colordiff * files are formatted on a thread pool, not a process pool: b.py loads tool modules under a name absent from sys.modules, so a forked child cannot unpickle the work function. The work is all subprocess calls, so threads lose nothing * no licence-header formatter (commented out upstream) and therefore no pygit2 * no eslint/prettier, as there is no JavaScript in this repo .cmake-format.py drops NUbots' custom-command declarations and wraps k1sim_role() instead of nuclear_role(). mujoco/idl_gen/ is excluded from clang-format: it is fastddsgen output that the build regenerates. Aligning black/isort with NUbots also lifts the requires-python <3.13 cap, which only existed to accommodate black 20.8b1. This commit adds the tooling only; the reformat is the commit that follows. Co-Authored-By: Claude Opus 5 --- .clang-format | 127 +++++++++++++++++++++ .cmake-format.py | 246 ++++++++++++++++++++++++++++++++++++++++ docs/K1_MUJOCO_SETUP.md | 16 +++ mujoco/tools/format.py | 226 ++++++++++++++++++++++++++++++++++++ pyproject.toml | 25 ++-- uv.lock | 176 ++++++++++------------------ 6 files changed, 690 insertions(+), 126 deletions(-) create mode 100644 .clang-format create mode 100644 .cmake-format.py create mode 100644 mujoco/tools/format.py diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..e21fda0 --- /dev/null +++ b/.clang-format @@ -0,0 +1,127 @@ +--- +# C++ files +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveMacros: true +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: false +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Empty +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: Never +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: true + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeBraces: Custom +BreakInheritanceList: BeforeComma +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeComma +BreakStringLiterals: true +ColumnLimit: 120 +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +FixNamespaceComments: true +IncludeBlocks: Regroup +IncludeCategories: + # System includes + - Regex: "^<.*>" + Priority: 1 + # Extensions + - Regex: '^"extension/.*"' + Priority: 100 + # Messages + - Regex: '^"message/.*"' + Priority: 101 + # Utilities + - Regex: '^"utility/.*"' + Priority: 102 + # Other headers + - Regex: '^".*"' + Priority: 2 +IndentCaseLabels: true +IndentPPDirectives: BeforeHash +IndentWrappedFunctionNames: true +IndentWidth: 4 +KeepEmptyLinesAtTheStartOfBlocks: true +MaxEmptyLinesToKeep: 2 +NamespaceIndentation: All +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: true +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInCStyleCastParentheses: false +SpacesInContainerLiterals: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never + +--- +# Protocol buffers +Language: Proto +BasedOnStyle: Google +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: true +AlignEscapedNewlinesLeft: false +AlignOperands: true +AlignTrailingComments: true +AllowShortBlocksOnASingleLine: false +ColumnLimit: 120 +IndentWidth: 4 +KeepEmptyLinesAtTheStartOfBlocks: true +MaxEmptyLinesToKeep: 2 +ReflowComments: true +SortIncludes: true +SpaceBeforeAssignmentOperators: true +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +TabWidth: 4 +UseTab: Never diff --git a/.cmake-format.py b/.cmake-format.py new file mode 100644 index 0000000..c52ad73 --- /dev/null +++ b/.cmake-format.py @@ -0,0 +1,246 @@ +# +# MIT License +# +# Copyright (c) 2019 NUbots +# +# This file is part of the NUSim codebase. +# See https://github.com/NUbots/NUSim for further info. +# +# Imported from NUbots (https://github.com/NUbots/NUbots) so that CMake files format +# identically in both repos; only the custom-command entries below differ. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# + +# ---------------------------------- +# Options affecting listfile parsing +# ---------------------------------- +with section("parse"): + + # Specify structure for custom cmake functions + additional_commands = {} + + # Specify variable tags. + vartags = [] + + # Specify property tags. + proptags = [] + +# ----------------------------- +# Options affecting formatting. +# ----------------------------- +with section("format"): + + # How wide to allow formatted cmake files + line_width = 120 + + # How many spaces to tab for indent + tab_size = 2 + + # If an argument group contains more than this many sub-groups (parg or kwarg + # groups) then force it to a vertical layout. + max_subgroups_hwrap = 2 + + # If a positional argument group contains more than this many arguments, then + # force it to a vertical layout. + max_pargs_hwrap = 6 + + # If a cmdline positional group consumes more than this many lines without + # nesting, then invalidate the layout (and nest) + max_rows_cmdline = 2 + + # If true, separate flow control names from their parentheses with a space + separate_ctrl_name_with_space = False + + # If true, separate function names from parentheses with a space + separate_fn_name_with_space = False + + # If a statement is wrapped to more than one line, than dangle the closing + # parenthesis on its own line. + dangle_parens = True + + # If the trailing parenthesis must be 'dangled' on its on line, then align it + # to this reference: `prefix`: the start of the statement, `prefix-indent`: + # the start of the statement, plus one indentation level, `child`: align to + # the column of the arguments + dangle_align = "prefix" + + # If the statement spelling length (including space and parenthesis) is + # smaller than this amount, then force reject nested layouts. + min_prefix_chars = 4 + + # If the statement spelling length (including space and parenthesis) is larger + # than the tab width by more than this amount, then force reject un-nested + # layouts. + max_prefix_chars = 10 + + # If a candidate layout is wrapped horizontally but it exceeds this many + # lines, then reject the layout. + max_lines_hwrap = 2 + + # What style line endings to use in the output. + line_ending = "unix" + + # Format command names consistently as 'lower' or 'upper' case + command_case = "canonical" + + # Format keywords consistently as 'lower' or 'upper' case + keyword_case = "upper" + + # A list of command names which should always be wrapped + always_wrap = ["K1SIM_ROLE", "k1sim_role"] + + # If true, the argument lists which are known to be sortable will be sorted + # lexicographicall + enable_sort = True + + # If true, the parsers may infer whether or not an argument list is sortable + # (without annotation). + autosort = True + + # By default, if cmake-format cannot successfully fit everything into the + # desired linewidth it will apply the last, most agressive attempt that it + # made. If this flag is True, however, cmake-format will print error, exit + # with non-zero status code, and write-out nothing + require_valid_layout = False + + # A dictionary mapping layout nodes to a list of wrap decisions. See the + # documentation for more information. + layout_passes = {} + +# ------------------------------------------------ +# Options affecting comment reflow and formatting. +# ------------------------------------------------ +with section("markup"): + + # What character to use for bulleted lists + bullet_char = "*" + + # What character to use as punctuation after numerals in an enumerated list + enum_char = "." + + # If comment markup is enabled, don't reflow the first comment block in each + # listfile. Use this to preserve formatting of your copyright/license + # statements. + first_comment_is_literal = False + + # If comment markup is enabled, don't reflow any comment block which matches + # this (regex) pattern. Default is `None` (disabled). + literal_comment_pattern = None + + # Regular expression to match preformat fences in comments default= + # ``r'^\s*([`~]{3}[`~]*)(.*)$'`` + fence_pattern = "^\\s*([`~]{3}[`~]*)(.*)$" + + # Regular expression to match rulers in comments default= + # ``r'^\s*[^\w\s]{3}.*[^\w\s]{3}$'`` + ruler_pattern = "^\\s*[^\\w\\s]{3}.*[^\\w\\s]{3}$" + + # If a comment line matches starts with this pattern then it is explicitly a + # trailing comment for the preceeding argument. Default is '#<' + explicit_trailing_pattern = "#<" + + # If a comment line starts with at least this many consecutive hash + # characters, then don't lstrip() them off. This allows for lazy hash rulers + # where the first hash char is not separated by space + hashruler_min_length = 10 + + # If true, then insert a space between the first hash char and remaining hash + # chars in a hash ruler, and normalize its length to fill the column + canonicalize_hashrulers = True + + # enable comment markup parsing and reflow + enable_markup = True + +# ---------------------------- +# Options affecting the linter +# ---------------------------- +with section("lint"): + + # a list of lint codes to disable + disabled_codes = [] + + # regular expression pattern describing valid function names + function_pattern = "[0-9a-z_]+" + + # regular expression pattern describing valid macro names + macro_pattern = "[0-9A-Z_]+" + + # regular expression pattern describing valid names for variables with global + # scope + global_var_pattern = "[0-9A-Z][0-9A-Z_]+" + + # regular expression pattern describing valid names for variables with global + # scope (but internal semantic) + internal_var_pattern = "_[0-9A-Z][0-9A-Z_]+" + + # regular expression pattern describing valid names for variables with local + # scope + local_var_pattern = "[0-9a-z_]+" + + # regular expression pattern describing valid names for privatedirectory + # variables + private_var_pattern = "_[0-9a-z_]+" + + # regular expression pattern describing valid names for publicdirectory + # variables + public_var_pattern = "[0-9A-Z][0-9A-Z_]+" + + # regular expression pattern describing valid names for keywords used in + # functions or macros + keyword_pattern = "[0-9A-Z_]+" + + # In the heuristic for C0201, how many conditionals to match within a loop in + # before considering the loop a parser. + max_conditionals_custom_parser = 2 + + # Require at least this many newlines between statements + min_statement_spacing = 1 + + # Require no more than this many newlines between statements + max_statement_spacing = 1 + max_returns = 6 + max_branches = 12 + max_arguments = 5 + max_localvars = 15 + max_statements = 50 + +# ------------------------------- +# Options affecting file encoding +# ------------------------------- +with section("encode"): + + # If true, emit the unicode byte-order mark (BOM) at the start of the file + emit_byteorder_mark = False + + # Specify the encoding of the input file. Defaults to utf-8 + input_encoding = "utf-8" + + # Specify the encoding of the output file. Defaults to utf-8. Note that cmake + # only claims to support utf-8 so be careful when using anything else + output_encoding = "utf-8" + +# ------------------------------------- +# Miscellaneous configurations options. +# ------------------------------------- +with section("misc"): + + # A dictionary containing any per-command configuration overrides. Currently + # only `command_case` is supported. + per_command = {} diff --git a/docs/K1_MUJOCO_SETUP.md b/docs/K1_MUJOCO_SETUP.md index f7e3ecf..4e2c65e 100644 --- a/docs/K1_MUJOCO_SETUP.md +++ b/docs/K1_MUJOCO_SETUP.md @@ -179,6 +179,22 @@ The C++ deploy MuJoCo version is pinned in `cmake/MuJoCoTarget.cmake`, `docker/D `tools/install_deps.sh`. Keep the training side (the mujoco_playground fork, §7) on the same MuJoCo version to avoid a sim2sim gap; bumping one means bumping **all** and rebuilding the image (`./b image`). +### Formatting + +`./b format` runs the same formatters, at the same pinned versions and against the same configs, as +NUbots: clang-format 14.0.6 (`.clang-format`), cmake-format (`.cmake-format.py`), isort and black. A +given file therefore formats identically in either repo. + +```bash +./b format # files that differ from origin/main +./b format --all # every tracked file +./b format --check # print a diff instead of writing; exits 1 if anything differs +./b format '*.cpp' # limit to a glob +``` + +It runs on the host out of the uv environment (`uv sync` happens automatically), not in the docker +image. `mujoco/idl_gen/` is excluded: it is fastddsgen output, regenerated by the build. + ### Extra `./b` commands - **`./b image`** — (re)build the docker toolchain image. `./b build` only builds it when it's *missing*, so diff --git a/mujoco/tools/format.py b/mujoco/tools/format.py new file mode 100644 index 0000000..2c7b999 --- /dev/null +++ b/mujoco/tools/format.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +# +# MIT License +# +# Copyright (c) 2017 NUbots +# +# This file is part of the NUSim codebase. +# See https://github.com/NUbots/NUSim for further info. +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# +"""./b format — run the formatters over the codebase (a port of NUbots' tools/format.py). + + ./b format format files that differ from origin/main + ./b format --all format every tracked file + ./b format --check print a diff instead of writing, exit 1 if anything differs + ./b format '*.cpp' limit to files matching a glob + +Differences from the NUbots original, all forced by NUSim's plumbing rather than taste: + * the formatters run on the host out of the uv project environment, not inside the + docker image (NUSim's ./b is a host-side stdlib-only dispatcher, see mujoco/b.py), + so there is no @run_on_docker decorator + * files are formatted on a thread pool rather than a process pool: b.py loads tool + modules under a name that is not in sys.modules, so a forked child cannot unpickle + the work function. The work is all subprocess calls, so threads lose nothing + * --check diffs with difflib instead of shelling out to colordiff, which is not a + dependency NUSim otherwise has + * no licence-header formatter (it is commented out upstream) and therefore no pygit2 + * no eslint/prettier (no JavaScript in this repo) + +The formatter versions are pinned in pyproject.toml to the ones NUbots uses, so a given +file formats identically in both repos. +""" +import difflib +import os +import shutil +import sys +import tempfile +from collections import OrderedDict +from concurrent.futures import ThreadPoolExecutor +from fnmatch import fnmatch +from subprocess import DEVNULL, PIPE, STDOUT, CalledProcessError +from subprocess import run as sp_run + +import b + +VENV_BIN = os.path.join(b.repo_dir, ".venv", "bin") + + +def _sync_env(): + """Create/update the uv project environment holding the pinned formatters. + + --project is required: uv >= 0.12 otherwise walks up from the cwd and can pick a + different pyproject.toml, landing in an environment where the tools are missing. + """ + if not os.path.isdir(VENV_BIN): + print("Setting up the formatter environment (uv sync)...") + sp_run(["uv", "sync", "--project", b.repo_dir, "--quiet"], check=True, stdout=DEVNULL) + + +def _tool(name): + """Absolute path to a pinned formatter, so we don't pay uv's startup cost per file.""" + path = os.path.join(VENV_BIN, name) + if os.path.isfile(path): + return path + found = shutil.which(name) + if found is None: + sys.exit(f"error: {name} not found. Run `uv sync` in {b.repo_dir}.") + return found + +# The extensions that are handled by the various formatters +formatters = OrderedDict() +formatters["clang-format"] = { + "format": [["clang-format", "-i", "-style=file", "{path}"]], + "include": ["*.h", "*.c", "*.cc", "*.cxx", "*.cpp", "*.hpp", "*.ipp", "*.frag", "*.glsl", "*.vert", "*.proto"], + # idl_gen/ is fastddsgen output, regenerated on every build (see cmake/IdlGen.cmake); + # formatting it would be overwritten and would obscure diffs against the generator. + "exclude": ["mujoco/idl_gen/*", "**/idl_gen/*"], +} +formatters["cmake-format"] = { + "format": [["cmake-format", "--in-place", "{path}"]], + "include": ["*.cmake", "*.role", "CMakeLists.txt", "**/CMakeLists.txt"], + "exclude": [], +} +formatters["isort"] = { + "format": [["isort", "--quiet", "{path}"]], + "include": ["*.py"], + "exclude": [], +} +formatters["black"] = { + "format": [["black", "--quiet", "{path}"]], + "include": ["*.py"], + "exclude": [], +} + + +def _do_format(path, verbose, check=True): + """Format one file in a temp copy; either diff it against the original or replace it.""" + text = "" + success = True + try: + # Find the correct formatter and format the file + formatter = [] + formatter_names = [] + for name, fmt in formatters.items(): + if (any(fnmatch(path, pattern) for pattern in fmt["include"])) and ( + all(not fnmatch(path, pattern) for pattern in fmt["exclude"]) + ): + formatter_names.append(name) + formatter.extend(fmt["format"]) + + # If we don't have a formatter then skip this file + if len(formatter) == 0: + return f"Skipping {path} as it does not match any of the formatters\n" if verbose >= 1 else "", True + + text = f"Formatting {path} with {', '.join(formatter_names)}\n" + + # Format a copy, so a formatter that fails half way can't leave the file mangled. + with tempfile.TemporaryDirectory(dir=os.path.dirname(path) or ".") as tmp_dir: + output_path = os.path.join(tmp_dir, os.path.basename(path)) + shutil.copy(path, output_path) + + tool_text = "" + for c in formatter: + cmd = [_tool(c[0])] + [arg.format(path=output_path) for arg in c[1:]] + if verbose >= 2: + text += f"\t$ {' '.join(cmd)}\n" + tool_text += sp_run(cmd, stderr=STDOUT, stdout=PIPE, check=True).stdout.decode("utf-8") + + if verbose >= 1 and tool_text: + text += tool_text + + with open(path, "r", encoding="utf-8", errors="replace") as f: + original = f.readlines() + with open(output_path, "r", encoding="utf-8", errors="replace") as f: + formatted = f.readlines() + + if original == formatted: + return ("" if verbose == 0 else text), True + + if check: + text += "".join(difflib.unified_diff(original, formatted, fromfile=path, tofile=f"{path} (formatted)")) + success = False + else: + shutil.copy(output_path, path) + + except CalledProcessError as e: + text += e.output.decode("utf-8").strip() + success = False + + return text, success + + +def register(command): + command.description = "Format the code in the codebase (clang-format, cmake-format, isort, black)" + + command.add_argument("-v", "--verbose", action="count", default=0, help="Print the output of the formatters") + command.add_argument( + "-a", + "--all", + dest="format_all", + action="store_true", + default=False, + help="Include unmodified files, as well as modified files, compared to main.", + ) + command.add_argument( + "-c", + "--check", + dest="check", + action="store_true", + default=False, + help="Check that files conform to formatting requirements", + ) + command.add_argument("globs", nargs="*", help="Globs with which to limit the files to format") + + +def run(verbose, check, format_all, globs, **kwargs): + os.chdir(b.repo_dir) + _sync_env() + + # Every tracked file, or just the ones that differ from main. + if format_all: + files = sp_run(["git", "ls-files"], stdout=PIPE, check=True).stdout.decode("utf-8").splitlines() + else: + base = "origin/main" if _has_ref("origin/main") else "main" + files = ( + sp_run(["git", "diff", "--name-only", base], stdout=PIPE, check=True).stdout.decode("utf-8").splitlines() + ) + + # git diff can name deleted files + files = [f for f in files if os.path.isfile(f)] + + if len(globs) != 0: + files = [f for f in files if any(fnmatch(f, g) for g in globs)] + + if not files: + print("No files to format") + return + + success = True + with ThreadPoolExecutor(max_workers=os.cpu_count()) as pool: + for r, s in pool.map(lambda f: _do_format(f, verbose=verbose, check=check), files): + sys.stdout.write(r) + success = success and s + + sys.exit(0 if success else 1) + + +def _has_ref(ref): + return sp_run(["git", "rev-parse", "--verify", "--quiet", ref], stdout=PIPE).returncode == 0 diff --git a/pyproject.toml b/pyproject.toml index 5d57a08..79269d0 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -3,17 +3,17 @@ name = "nusim" version = "0.1.0" description = "Host-side tooling (the ./b dispatcher + formatters) for the NUSim K1 MuJoCo simulator." readme = "README.md" -# Cap <3.13: pinned black 20.8b1 pulls typed-ast, no 3.13 wheel (lift once black is -# bumped). uv fetches a matching interpreter. -requires-python = ">=3.11,<3.13" +requires-python = ">=3.11" +# Formatter versions are pinned to the ones NUbots uses, so the same file formats +# identically in both repos. clang-format is the PyPI wheel of the same LLVM release +# NUbots builds into its docker image (llvmorg-14.0.6) — clang-format output changes +# between major versions, so this pin is what keeps ./b format reproducible. dependencies = [ - "cmake-format==0.6.13", - "isort==5.7.0", - "black==20.8b1", - # black 20.8b1 imports click._unicodefun, removed in click 8.1. Drop this pin - # when black is bumped. - "click<8.1", - "termcolor", + "clang-format==14.0.6", + "cmakelang==0.6.13", + "isort==5.13.2", + "black==24.10.0", + "termcolor==2.5.0", ] [tool.uv] @@ -24,5 +24,6 @@ package = false line-length = 120 [tool.isort] -line_length=120 -known_first_party=['b', 'utility'] +profile = "black" +line_length = 120 +known_first_party = ['b'] diff --git a/uv.lock b/uv.lock index e36624c..4b043f1 100644 --- a/uv.lock +++ b/uv.lock @@ -1,35 +1,50 @@ version = 1 revision = 3 -requires-python = ">=3.11, <3.13" -resolution-markers = [ - "python_full_version >= '3.12'", - "python_full_version < '3.12'", -] - -[[package]] -name = "appdirs" -version = "1.4.4" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/d7/d8/05696357e0311f5b5c316d7b95f46c669dd9c15aaeecbb48c7d0aeb88c40/appdirs-1.4.4.tar.gz", hash = "sha256:7d5d0167b2b1ba821647616af46a749d1c653740dd0d2415100fe26e27afdf41", size = 13470, upload-time = "2020-05-11T07:59:51.037Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/3b/00/2344469e2084fb287c2e0b57b72910309874c3245463acd6cf5e3db69324/appdirs-1.4.4-py2.py3-none-any.whl", hash = "sha256:a841dacd6b99318a741b166adb07e19ee71a274450e68237b4650ca1055ab128", size = 9566, upload-time = "2020-05-11T07:59:49.499Z" }, -] +requires-python = ">=3.11" [[package]] name = "black" -version = "20.8b1" +version = "24.10.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "appdirs" }, { name = "click" }, { name = "mypy-extensions" }, + { name = "packaging" }, { name = "pathspec" }, - { name = "regex" }, - { name = "toml" }, - { name = "typed-ast" }, - { name = "typing-extensions" }, + { name = "platformdirs" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/d8/0d/cc2fb42b8c50d80143221515dd7e4766995bd07c56c9a3ed30baf080b6dc/black-24.10.0.tar.gz", hash = "sha256:846ea64c97afe3bc677b761787993be4991810ecc7a4a937816dd6bddedc4875", size = 645813, upload-time = "2024-10-07T19:20:50.361Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/c2/cc/7496bb63a9b06a954d3d0ac9fe7a73f3bf1cd92d7a58877c27f4ad1e9d41/black-24.10.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:5a2221696a8224e335c28816a9d331a6c2ae15a2ee34ec857dcf3e45dbfa99ad", size = 1607468, upload-time = "2024-10-07T19:26:14.966Z" }, + { url = "https://files.pythonhosted.org/packages/2b/e3/69a738fb5ba18b5422f50b4f143544c664d7da40f09c13969b2fd52900e0/black-24.10.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:f9da3333530dbcecc1be13e69c250ed8dfa67f43c4005fb537bb426e19200d50", size = 1437270, upload-time = "2024-10-07T19:25:24.291Z" }, + { url = "https://files.pythonhosted.org/packages/c9/9b/2db8045b45844665c720dcfe292fdaf2e49825810c0103e1191515fc101a/black-24.10.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:4007b1393d902b48b36958a216c20c4482f601569d19ed1df294a496eb366392", size = 1737061, upload-time = "2024-10-07T19:23:52.18Z" }, + { url = "https://files.pythonhosted.org/packages/a3/95/17d4a09a5be5f8c65aa4a361444d95edc45def0de887810f508d3f65db7a/black-24.10.0-cp311-cp311-win_amd64.whl", hash = "sha256:394d4ddc64782e51153eadcaaca95144ac4c35e27ef9b0a42e121ae7e57a9175", size = 1423293, upload-time = "2024-10-07T19:24:41.7Z" }, + { url = "https://files.pythonhosted.org/packages/90/04/bf74c71f592bcd761610bbf67e23e6a3cff824780761f536512437f1e655/black-24.10.0-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:b5e39e0fae001df40f95bd8cc36b9165c5e2ea88900167bddf258bacef9bbdc3", size = 1644256, upload-time = "2024-10-07T19:27:53.355Z" }, + { url = "https://files.pythonhosted.org/packages/4c/ea/a77bab4cf1887f4b2e0bce5516ea0b3ff7d04ba96af21d65024629afedb6/black-24.10.0-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:d37d422772111794b26757c5b55a3eade028aa3fde43121ab7b673d050949d65", size = 1448534, upload-time = "2024-10-07T19:26:44.953Z" }, + { url = "https://files.pythonhosted.org/packages/4e/3e/443ef8bc1fbda78e61f79157f303893f3fddf19ca3c8989b163eb3469a12/black-24.10.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:14b3502784f09ce2443830e3133dacf2c0110d45191ed470ecb04d0f5f6fcb0f", size = 1761892, upload-time = "2024-10-07T19:24:10.264Z" }, + { url = "https://files.pythonhosted.org/packages/52/93/eac95ff229049a6901bc84fec6908a5124b8a0b7c26ea766b3b8a5debd22/black-24.10.0-cp312-cp312-win_amd64.whl", hash = "sha256:30d2c30dc5139211dda799758559d1b049f7f14c580c409d6ad925b74a4208a8", size = 1434796, upload-time = "2024-10-07T19:25:06.239Z" }, + { url = "https://files.pythonhosted.org/packages/d0/a0/a993f58d4ecfba035e61fca4e9f64a2ecae838fc9f33ab798c62173ed75c/black-24.10.0-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:1cbacacb19e922a1d75ef2b6ccaefcd6e93a2c05ede32f06a21386a04cedb981", size = 1643986, upload-time = "2024-10-07T19:28:50.684Z" }, + { url = "https://files.pythonhosted.org/packages/37/d5/602d0ef5dfcace3fb4f79c436762f130abd9ee8d950fa2abdbf8bbc555e0/black-24.10.0-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:1f93102e0c5bb3907451063e08b9876dbeac810e7da5a8bfb7aeb5a9ef89066b", size = 1448085, upload-time = "2024-10-07T19:28:12.093Z" }, + { url = "https://files.pythonhosted.org/packages/47/6d/a3a239e938960df1a662b93d6230d4f3e9b4a22982d060fc38c42f45a56b/black-24.10.0-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ddacb691cdcdf77b96f549cf9591701d8db36b2f19519373d60d31746068dbf2", size = 1760928, upload-time = "2024-10-07T19:24:15.233Z" }, + { url = "https://files.pythonhosted.org/packages/dd/cf/af018e13b0eddfb434df4d9cd1b2b7892bab119f7a20123e93f6910982e8/black-24.10.0-cp313-cp313-win_amd64.whl", hash = "sha256:680359d932801c76d2e9c9068d05c6b107f2584b2a5b88831c83962eb9984c1b", size = 1436875, upload-time = "2024-10-07T19:24:42.762Z" }, + { url = "https://files.pythonhosted.org/packages/8d/a7/4b27c50537ebca8bec139b872861f9d2bf501c5ec51fcf897cb924d9e264/black-24.10.0-py3-none-any.whl", hash = "sha256:3bb2b7a1f7b685f85b11fed1ef10f8a9148bceb49853e47a294a3dd963c1dd7d", size = 206898, upload-time = "2024-10-07T19:20:48.317Z" }, +] + +[[package]] +name = "clang-format" +version = "14.0.6" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/0c/92/d57c1b3ea310ae0f48ab51a5aa2c87c4c732c3d79037ad2527f2eed7ca34/clang-format-14.0.6.tar.gz", hash = "sha256:d5c96b500d7f8b5d2db5b75ac035be387512850ad589cdc3019666b861382136", size = 9598, upload-time = "2022-06-27T11:34:36.46Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/08/62/71ffc9213f66cab7dd5adc5e933b5f64323272c197fcff2905674016c03d/clang_format-14.0.6-py2.py3-none-macosx_10_9_universal2.whl", hash = "sha256:bd400c47665dd19afc03f98e747f78ed828abab99c6a1b07e137b35c1cd3cc26", size = 1016919, upload-time = "2022-06-27T11:33:40.936Z" }, + { url = "https://files.pythonhosted.org/packages/5f/de/f666633c30a4cc9e987d153db992849bfeea03ad200bf1cfa937039c64ff/clang_format-14.0.6-py2.py3-none-manylinux_2_12_i686.manylinux2010_i686.whl", hash = "sha256:13f2d6d4a2af004a783c65f0921afa8f0384bffcdaf500b6c2cb542edeb0b4a5", size = 1259649, upload-time = "2022-06-27T11:34:01.64Z" }, + { url = "https://files.pythonhosted.org/packages/ce/27/df41404419d9116e071d0b8a5ba0a0969d9db7587af689ec81ec75c1f18a/clang_format-14.0.6-py2.py3-none-manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:d7c1c5e404c58e55f0170f01b3c5611dce6c119e62b5d1020347e0ad97d5a047", size = 1147591, upload-time = "2022-06-27T11:34:08.688Z" }, + { url = "https://files.pythonhosted.org/packages/23/e4/ea55429601432913e9fe40686c3c09a79338075c830a523fabc71aa49c69/clang_format-14.0.6-py2.py3-none-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:dbfd60528eb3bb7d7cfe8576faa70845fbf93601f815ef75163d36606e87f388", size = 1205157, upload-time = "2022-06-27T11:34:13.842Z" }, + { url = "https://files.pythonhosted.org/packages/8c/67/e1faf73ea166669e1698f55f3ae366369db57d75eb3b6c04c93620ebac12/clang_format-14.0.6-py2.py3-none-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:c93580945f75de7e01996f1fb3cf67e4dc424f1c864e237c85614fb99a48c7a4", size = 1949067, upload-time = "2022-06-27T11:34:18.984Z" }, + { url = "https://files.pythonhosted.org/packages/cd/3b/3e20072464e98314eafdc5bc5744454ade6e6f5e525fb29f6b4555173811/clang_format-14.0.6-py2.py3-none-manylinux_2_17_s390x.manylinux2014_s390x.whl", hash = "sha256:aaf4edecc46a24f0b572b82cf5827e292ad1c137903427627c4d5f671668cc2b", size = 1187836, upload-time = "2022-06-27T11:34:23.88Z" }, + { url = "https://files.pythonhosted.org/packages/6e/06/302903004246dd62a11965e9f672b975c58ad6966985dbcaa14c6cdb4779/clang_format-14.0.6-py2.py3-none-win32.whl", hash = "sha256:810c649ab97d208cd418c897d50ab6e958eb8d96854527edd80d0dd21a75e914", size = 833512, upload-time = "2022-06-27T11:34:27.769Z" }, + { url = "https://files.pythonhosted.org/packages/63/7a/1f11404d5097263ad065cf9166dd00be0a8c1040c1ec4f57921ac07591eb/clang_format-14.0.6-py2.py3-none-win_amd64.whl", hash = "sha256:d780c04334bca80f2b60d25bf53c37bd0618520ee295a7888a11f25bde114ac4", size = 1007035, upload-time = "2022-06-27T11:34:32.164Z" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/dc/7b/5a6bbe89de849f28d7c109f5ea87b65afa5124ad615f3419e71beb29dc96/black-20.8b1.tar.gz", hash = "sha256:1c02557aa099101b9d21496f8a914e9ed2222ef70336404eeeac8edba836fbea", size = 1096433, upload-time = "2020-08-26T15:52:20.948Z" } [[package]] name = "click" @@ -43,18 +58,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/4a/a8/0b2ced25639fb20cc1c9784de90a8c25f9504a7f18cd8b5397bd61696d7d/click-8.0.4-py3-none-any.whl", hash = "sha256:6a7a62563bbfabfda3a38f3023a1db4a35978c0abd76f6c9605ecd6554d6d9b1", size = 97486, upload-time = "2022-02-18T20:31:27.733Z" }, ] -[[package]] -name = "cmake-format" -version = "0.6.13" -source = { registry = "https://pypi.org/simple" } -dependencies = [ - { name = "cmakelang" }, -] -sdist = { url = "https://files.pythonhosted.org/packages/5d/3e/f1a7388ce2528b69fbfce9691e5c97f1b2b2b83c25711e0627c3e8f99e58/cmake-format-0.6.13.tar.gz", hash = "sha256:1a48b779067ecca68c498691d07d5c9d3df9803a7e0c5b641128fa6efe5ae489", size = 10484, upload-time = "2020-08-19T17:15:26.286Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/4a/40/0ca7c62dc4b9af58ca32da8e6d87ee222f5bb551c17ef22900b2f81b998e/cmake_format-0.6.13-py3-none-any.whl", hash = "sha256:ec7ed949101e5f0b7bc19317d122b83ccbc28fd766c41c93094845719667c56e", size = 19725, upload-time = "2020-08-19T17:15:24.23Z" }, -] - [[package]] name = "cmakelang" version = "0.6.13" @@ -78,11 +81,11 @@ wheels = [ [[package]] name = "isort" -version = "5.7.0" +version = "5.13.2" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/a2/f7/f50fc9555dc0fe2dc1e7f69d93f71961d052857c296cad0fb6d275b20008/isort-5.7.0.tar.gz", hash = "sha256:c729845434366216d320e936b8ad6f9d681aab72dc7cbc2d51bedc3582f3ad1e", size = 169353, upload-time = "2020-12-31T01:15:43.126Z" } +sdist = { url = "https://files.pythonhosted.org/packages/87/f9/c1eb8635a24e87ade2efce21e3ce8cd6b8630bb685ddc9cdaca1349b2eb5/isort-5.13.2.tar.gz", hash = "sha256:48fdfcb9face5d58a4f6dde2e72a1fb8dcaf8ab26f95ab49fab84c2ddefb0109", size = 175303, upload-time = "2023-12-13T20:37:26.124Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/cc/89/6888f573886e9dc0906ec98f1b15888de20919a142c355d7f57ebd977d36/isort-5.7.0-py3-none-any.whl", hash = "sha256:fff4f0c04e1825522ce6949973e83110a6e907750cd92d128b0d14aaaadbffdc", size = 104240, upload-time = "2020-12-31T01:15:41.299Z" }, + { url = "https://files.pythonhosted.org/packages/d1/b3/8def84f539e7d2289a02f0524b944b15d7c75dab7628bedf1c4f0992029c/isort-5.13.2-py3-none-any.whl", hash = "sha256:8ca5e72a8d85860d5a3fa69b8745237f2939afe12dbf656afbcb47fe72d947a6", size = 92310, upload-time = "2023-12-13T20:37:23.244Z" }, ] [[package]] @@ -100,19 +103,28 @@ version = "0.1.0" source = { virtual = "." } dependencies = [ { name = "black" }, - { name = "click" }, - { name = "cmake-format" }, + { name = "clang-format" }, + { name = "cmakelang" }, { name = "isort" }, { name = "termcolor" }, ] [package.metadata] requires-dist = [ - { name = "black", specifier = "==20.8b1" }, - { name = "click", specifier = "<8.1" }, - { name = "cmake-format", specifier = "==0.6.13" }, - { name = "isort", specifier = "==5.7.0" }, - { name = "termcolor" }, + { name = "black", specifier = "==24.10.0" }, + { name = "clang-format", specifier = "==14.0.6" }, + { name = "cmakelang", specifier = "==0.6.13" }, + { name = "isort", specifier = "==5.13.2" }, + { name = "termcolor", specifier = "==2.5.0" }, +] + +[[package]] +name = "packaging" +version = "26.3" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/7d/fa/3944b40b07da9ce895c0e6303a5ab7d53da063554f534556b134a54d6093/packaging-26.3.tar.gz", hash = "sha256:94edc256424af38762eb31306eed28beb9f0efc50a8837492c9d6fd6004aed79", size = 313412, upload-time = "2026-08-04T18:15:28.737Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/63/34/ba1c580383c9eada3711951fef0795c80b829a078d72188184bcab9dd527/packaging-26.3-py3-none-any.whl", hash = "sha256:d7193f7c8e4e93f444fde0262bf90af30e16fa0ad0ad44cb553c87339b23cd1c", size = 129956, upload-time = "2026-08-04T18:15:27.159Z" }, ] [[package]] @@ -125,43 +137,12 @@ wheels = [ ] [[package]] -name = "regex" -version = "2026.6.28" +name = "platformdirs" +version = "4.11.8" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/f1/05/e4f219230e11e774a6c9987d2ab0d0c6b8573e13a17e143d0015bee710ef/regex-2026.6.28.tar.gz", hash = "sha256:3cb4b6c5cb3060cc31efdc1fbb27c25fb9b29044afd87e40601a1c4d9db54342", size = 416101, upload-time = "2026-06-28T19:56:55.302Z" } +sdist = { url = "https://files.pythonhosted.org/packages/53/18/f3bb8ef0d3b930692343da8aa4d3cbcd6749477c053959395ac81965a6e9/platformdirs-4.11.8.tar.gz", hash = "sha256:f23abafea7dd4276d1f29104b83598d7dcc567cafd07c9c951e66665645437fc", size = 37182, upload-time = "2026-09-08T22:20:42.866Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/72/db/9051b36294bdbabaa9c7db57db0fbcdfbd17f7a106c539bb423d0323faea/regex-2026.6.28-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:a71b51dd08b9b62f055fafab3dee8af8bd2ec81b373a44caef18d6c5ca28f43a", size = 489481, upload-time = "2026-06-28T19:53:36.684Z" }, - { url = "https://files.pythonhosted.org/packages/35/3f/24097a3c3ff30f9a639888900faaecabcf5f54a5bc9c851c297e11b349ef/regex-2026.6.28-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:9c26a47770d30a0f85c01e261d2a3ebc342c4af6fd666dbd8c1fe4cbf3adf726", size = 291292, upload-time = "2026-06-28T19:53:38.39Z" }, - { url = "https://files.pythonhosted.org/packages/5e/cc/e0d762a189cfb4e8926d16e691720690d139a977b38fdb80230c259332ab/regex-2026.6.28-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:e5efbc1af38f97e300d43028e5a92e752d924bcfb7f465d8669d5d5a6e78c233", size = 289232, upload-time = "2026-06-28T19:53:40.181Z" }, - { url = "https://files.pythonhosted.org/packages/4b/c8/ca0ac7f09cc88ca61e0c61c53f7db29334f660ffba5d0b52378e7c44723c/regex-2026.6.28-cp311-cp311-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:f1758df6fdd8c800620a5638958720e8a635e1da49a2f09df2dd63e94a24ec4a", size = 792332, upload-time = "2026-06-28T19:53:41.782Z" }, - { url = "https://files.pythonhosted.org/packages/8e/92/04ae94cbe0dd1f478b2aef6c46f995bb6946d3e338d4b28605478b66a2b7/regex-2026.6.28-cp311-cp311-manylinux2014_ppc64le.manylinux_2_17_ppc64le.manylinux_2_28_ppc64le.whl", hash = "sha256:ad73ecf20c1ef5c975639f8bf845a9370fcf7dada7edc1e3b0bca20e2f8202f6", size = 861743, upload-time = "2026-06-28T19:53:43.261Z" }, - { url = "https://files.pythonhosted.org/packages/4c/ec/024d7638c807679ff8a0e6081d01d66c7762339af1cac71e45911587ff9a/regex-2026.6.28-cp311-cp311-manylinux2014_s390x.manylinux_2_17_s390x.manylinux_2_28_s390x.whl", hash = "sha256:4d80c798b0eec6ea3d45f8816a1e8886c5664615d347d89e8c075b576a1b5a5d", size = 906481, upload-time = "2026-06-28T19:53:44.948Z" }, - { url = "https://files.pythonhosted.org/packages/cd/fd/93bfe5af45f0be4fa8983945455c0e6924e1aeb879cde227958869c1e71c/regex-2026.6.28-cp311-cp311-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:a361feeaf1b6ba1df060f2ff5c5947092edf537a35ce78e76387ac56d3e0f4a4", size = 799867, upload-time = "2026-06-28T19:53:46.997Z" }, - { url = "https://files.pythonhosted.org/packages/ee/fd/e5d965d41f2398c8ce0f37a4652f03bb297fd009bb796d390134225dda12/regex-2026.6.28-cp311-cp311-manylinux_2_31_riscv64.manylinux_2_39_riscv64.whl", hash = "sha256:8b92366d9c8bba9642989534073662abdd9b41faf7603a7ae71597833f3b88f0", size = 773632, upload-time = "2026-06-28T19:53:48.892Z" }, - { url = "https://files.pythonhosted.org/packages/eb/d9/ff39afaec92b9ee2dba0302a4783976005091681069808938c31cf8df3b6/regex-2026.6.28-cp311-cp311-musllinux_1_2_aarch64.whl", hash = "sha256:11251768cc23f097dd61b18f67966e70f74da822784d17e12a444eb6b29d4288", size = 781669, upload-time = "2026-06-28T19:53:50.693Z" }, - { url = "https://files.pythonhosted.org/packages/45/4e/e2fd4bb8228e10c24af2d7ff867182372190e498eab9fd29cbe54c403c95/regex-2026.6.28-cp311-cp311-musllinux_1_2_ppc64le.whl", hash = "sha256:ad5c67786145ec28a71a267d9f9d92bdc8d70d65541eea852c253f520a01f918", size = 854497, upload-time = "2026-06-28T19:53:52.323Z" }, - { url = "https://files.pythonhosted.org/packages/72/7c/f0340384a973082979064156d05f3d2cc1dced7371efcd7a1b45726a1a8a/regex-2026.6.28-cp311-cp311-musllinux_1_2_riscv64.whl", hash = "sha256:f1da438e739765c3e85175ede05816cbede3caaacb1e0680568bda6119bfdfca", size = 763335, upload-time = "2026-06-28T19:53:54.024Z" }, - { url = "https://files.pythonhosted.org/packages/e1/32/90ce0d0898e205506cc22b9c81cfb16b722e06ca5f50fad51c053c2a727b/regex-2026.6.28-cp311-cp311-musllinux_1_2_s390x.whl", hash = "sha256:d98b639046e51c5de64d9f77351532105e99ca271cb6f7640e1f903d6ab63032", size = 844615, upload-time = "2026-06-28T19:53:56.216Z" }, - { url = "https://files.pythonhosted.org/packages/6a/ef/55abb149599dce1ade687170557129524011eeb3d92afe02429cea7754a2/regex-2026.6.28-cp311-cp311-musllinux_1_2_x86_64.whl", hash = "sha256:1e164ace4dbab5c6ad4a4ac7c41a2638fe226d0c770a86f2eb041f594bac6ee7", size = 789193, upload-time = "2026-06-28T19:53:57.791Z" }, - { url = "https://files.pythonhosted.org/packages/f8/ea/cf7f6f6f152e52fdad978b913bf24c14df647eca0f81ef31f3aee0be8982/regex-2026.6.28-cp311-cp311-win32.whl", hash = "sha256:3169a3159e4d99d9ae85ff0ed90ef3b8906cc3152653b6078b842ace6c8f72c3", size = 266731, upload-time = "2026-06-28T19:53:59.938Z" }, - { url = "https://files.pythonhosted.org/packages/c6/cf/a48d8e8d406b22481cad146f48fa0dfca3c5f402b91f26d8e5a0fe4f513d/regex-2026.6.28-cp311-cp311-win_amd64.whl", hash = "sha256:5977295b0a74e8241df8a4b3b27b12412a831f6fa32ee8b755039592cd768c3d", size = 277918, upload-time = "2026-06-28T19:54:01.502Z" }, - { url = "https://files.pythonhosted.org/packages/89/b2/a222392207db7ed86281a732a99f7cf7f2bb35d332799e892b8510be000e/regex-2026.6.28-cp311-cp311-win_arm64.whl", hash = "sha256:f5fbaef40c3e9282ccee4b075f5600a0d858aa0c34147732f1baa69c8188a95d", size = 276876, upload-time = "2026-06-28T19:54:03.411Z" }, - { url = "https://files.pythonhosted.org/packages/da/21/44aa415873032056c43eac21c67285deb2cf66cddb2a964c3cdc8f803efc/regex-2026.6.28-cp312-cp312-macosx_10_13_universal2.whl", hash = "sha256:81cc5793ad33a10444445e8d29d3c73e752c8fb2e120772d70fcb6d41df40fe1", size = 490480, upload-time = "2026-06-28T19:54:05.392Z" }, - { url = "https://files.pythonhosted.org/packages/8b/5f/30d4116093c2128099f78b6990dfc1698fdbf3ee528f1e1c647378034c79/regex-2026.6.28-cp312-cp312-macosx_10_13_x86_64.whl", hash = "sha256:e18225243250a1f7d7e5e5d883f3b96465cd79031acf5c6db902b7025f2125d9", size = 292137, upload-time = "2026-06-28T19:54:07.088Z" }, - { url = "https://files.pythonhosted.org/packages/cb/0e/ca20a0e0de49837e6337603a91ab77556aa27033ac5b975615d98698cfb3/regex-2026.6.28-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:ecd1638b1c2db1f2d01c182a4b0d3e2e88b0e99910320a745c1727ee3638ddab", size = 289623, upload-time = "2026-06-28T19:54:08.762Z" }, - { url = "https://files.pythonhosted.org/packages/50/11/c013422a7e2c59946df8ac93e792a4922c98287f2a2181341603c78a5d98/regex-2026.6.28-cp312-cp312-manylinux2014_aarch64.manylinux_2_17_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:4303ebe16b74eeb3fe2715745023266fea92fd44a23f3e7bb2fb48c7a7bbc195", size = 796756, upload-time = "2026-06-28T19:54:10.616Z" }, - { url = "https://files.pythonhosted.org/packages/b0/95/1309645a0e1ee6fb91d954501da57a0b33d50ad2a9acb313702851a7054e/regex-2026.6.28-cp312-cp312-manylinux2014_ppc64le.manylinux_2_17_ppc64le.manylinux_2_28_ppc64le.whl", hash = "sha256:56b856b70b96c381d837f609eee442a1bd320cd2159f5c294b679552fb1a7eaf", size = 865465, upload-time = "2026-06-28T19:54:12.742Z" }, - { url = "https://files.pythonhosted.org/packages/20/06/491802db47c6f5e2904ffa2518ad3ac27fe6bbf5a66d73210a95cc080d47/regex-2026.6.28-cp312-cp312-manylinux2014_s390x.manylinux_2_17_s390x.manylinux_2_28_s390x.whl", hash = "sha256:f74675ab76ab1d005ffba4dee308e53e89efc22be6e9f9fae5b539a3f81bdff2", size = 912350, upload-time = "2026-06-28T19:54:14.508Z" }, - { url = "https://files.pythonhosted.org/packages/5e/60/3ba57840bcc7e2367090360de0c15a5ba6ad22be89314251105f2e943f43/regex-2026.6.28-cp312-cp312-manylinux2014_x86_64.manylinux_2_17_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:90581684565a93f7258af1e5d3f41ef20d7d7c61f2a428183a342bcb65485e38", size = 801261, upload-time = "2026-06-28T19:54:16.432Z" }, - { url = "https://files.pythonhosted.org/packages/eb/27/af1eb74e9a78c782b3e450b611a595e44906da8a5107e1227f4a7fd0480b/regex-2026.6.28-cp312-cp312-manylinux_2_31_riscv64.manylinux_2_39_riscv64.whl", hash = "sha256:28f9e6c28f9b90f6f784595a33240a57e181e61b6ee3dc259b25c61e356d1aa3", size = 777072, upload-time = "2026-06-28T19:54:18.128Z" }, - { url = "https://files.pythonhosted.org/packages/20/18/fdd4c883a39e3ed00d669062af1135809bfd3281bf528150849fbd68825b/regex-2026.6.28-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:378a71d861fc7c8806b04ac5b133d53c0e774f92f5d9663a539872d3fa2b0417", size = 785119, upload-time = "2026-06-28T19:54:20.314Z" }, - { url = "https://files.pythonhosted.org/packages/1c/79/0aabe34b8482dcadf64355f70f96e22eba5ec6c1efb33563f89654f4061c/regex-2026.6.28-cp312-cp312-musllinux_1_2_ppc64le.whl", hash = "sha256:4cc199874ecd6267a49b111052250825bfe19b5101b23b2ba80f54efa3e0994e", size = 860118, upload-time = "2026-06-28T19:54:22.368Z" }, - { url = "https://files.pythonhosted.org/packages/a8/2c/c973323306a27c9db7d160e9584eb7e0ece2a96224ccb0d39060558b31f9/regex-2026.6.28-cp312-cp312-musllinux_1_2_riscv64.whl", hash = "sha256:b916a10431494ef4b4d62c6c89cab6426af7873125b8cd6c15811bf5fc58eec8", size = 765786, upload-time = "2026-06-28T19:54:24.265Z" }, - { url = "https://files.pythonhosted.org/packages/e3/df/9ca3e378e352242a4cb45573a5e9162c3ee791507702a23966fa559e36b5/regex-2026.6.28-cp312-cp312-musllinux_1_2_s390x.whl", hash = "sha256:2e27727fba075f1e4409416d2f537d4c30fc11f012ea507f7bd74d3e19ecb57a", size = 852120, upload-time = "2026-06-28T19:54:25.972Z" }, - { url = "https://files.pythonhosted.org/packages/a2/3e/3e31e255c4971f53cbce6306b5e3c76cbd3735a54f419bb3b2f194e9f68c/regex-2026.6.28-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:700fc6a7844bb2c4149292ac79d1df8841a00acd4d45cd32c1ebc7bcc1fd0da8", size = 789503, upload-time = "2026-06-28T19:54:27.678Z" }, - { url = "https://files.pythonhosted.org/packages/72/01/d36561c21c3033d7eeb31d51b491916817de7861acefccc5fc9db8a5037c/regex-2026.6.28-cp312-cp312-win32.whl", hash = "sha256:03376d60b6a11aecb88a79fa2be06b40faa01c6693bc31ef69435cd4818b9463", size = 267109, upload-time = "2026-06-28T19:54:29.316Z" }, - { url = "https://files.pythonhosted.org/packages/a0/59/bbbb0591f38b18c65977cd65ce64749eba1c1996c99ac04e900fc30c0dcb/regex-2026.6.28-cp312-cp312-win_amd64.whl", hash = "sha256:fbd2ded482bf99e6651992bbfcde460272724d4bbc49ef3d6b46d9312867ec84", size = 277711, upload-time = "2026-06-28T19:54:31.143Z" }, - { url = "https://files.pythonhosted.org/packages/86/06/be4f6b337d773ae5739a1bc238f97c16926e72017243735853c030f4c628/regex-2026.6.28-cp312-cp312-win_arm64.whl", hash = "sha256:37294d3d7ddb64c7e89184b2894e0f8f0a19c514bc59513d71fe692c3a8d5fc6", size = 277022, upload-time = "2026-06-28T19:54:32.97Z" }, + { url = "https://files.pythonhosted.org/packages/f4/e1/5b7b8bbb55084d1425bcb9bc823ff519e1b2be05f6ebb0089e2eacc38413/platformdirs-4.11.8-py3-none-any.whl", hash = "sha256:52f2f181bbfde907966932cc8312d967d02976422d66d537ea16092b8e291081", size = 24027, upload-time = "2026-09-08T22:20:41.537Z" }, ] [[package]] @@ -175,42 +156,9 @@ wheels = [ [[package]] name = "termcolor" -version = "3.3.0" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/46/79/cf31d7a93a8fdc6aa0fbb665be84426a8c5a557d9240b6239e9e11e35fc5/termcolor-3.3.0.tar.gz", hash = "sha256:348871ca648ec6a9a983a13ab626c0acce02f515b9e1983332b17af7979521c5", size = 14434, upload-time = "2025-12-29T12:55:21.882Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/33/d1/8bb87d21e9aeb323cc03034f5eaf2c8f69841e40e4853c2627edf8111ed3/termcolor-3.3.0-py3-none-any.whl", hash = "sha256:cf642efadaf0a8ebbbf4bc7a31cec2f9b5f21a9f726f4ccbb08192c9c26f43a5", size = 7734, upload-time = "2025-12-29T12:55:20.718Z" }, -] - -[[package]] -name = "toml" -version = "0.10.2" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/be/ba/1f744cdc819428fc6b5084ec34d9b30660f6f9daaf70eead706e3203ec3c/toml-0.10.2.tar.gz", hash = "sha256:b3bda1d108d5dd99f4a20d24d9c348e91c4db7ab1b749200bded2f839ccbe68f", size = 22253, upload-time = "2020-11-01T01:40:22.204Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/44/6f/7120676b6d73228c96e17f1f794d8ab046fc910d781c8d151120c3f1569e/toml-0.10.2-py2.py3-none-any.whl", hash = "sha256:806143ae5bfb6a3c6e736a764057db0e6a0e05e338b5630894a5f779cabb4f9b", size = 16588, upload-time = "2020-11-01T01:40:20.672Z" }, -] - -[[package]] -name = "typed-ast" -version = "1.5.5" -source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/f9/7e/a424029f350aa8078b75fd0d360a787a273ca753a678d1104c5fa4f3072a/typed_ast-1.5.5.tar.gz", hash = "sha256:94282f7a354f36ef5dbce0ef3467ebf6a258e370ab33d5b40c249fa996e590dd", size = 252841, upload-time = "2023-07-04T18:38:08.524Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/75/53/b685e10da535c7b3572735f8bea0d4abb35a04722a7d44ca9c163a0cf822/typed_ast-1.5.5-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:c631da9710271cb67b08bd3f3813b7af7f4c69c319b75475436fcab8c3d21bee", size = 223264, upload-time = "2023-07-04T18:37:13.637Z" }, - { url = "https://files.pythonhosted.org/packages/96/fd/fc8ccf19fc16a40a23e7c7802d0abc78c1f38f1abb6e2447c474f8a076d8/typed_ast-1.5.5-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:b445c2abfecab89a932b20bd8261488d574591173d07827c1eda32c457358b18", size = 208158, upload-time = "2023-07-04T18:37:15.141Z" }, - { url = "https://files.pythonhosted.org/packages/bf/9a/598e47f2c3ecd19d7f1bb66854d0d3ba23ffd93c846448790a92524b0a8d/typed_ast-1.5.5-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:cc95ffaaab2be3b25eb938779e43f513e0e538a84dd14a5d844b8f2932593d88", size = 878366, upload-time = "2023-07-04T18:37:16.614Z" }, - { url = "https://files.pythonhosted.org/packages/60/ca/765e8bf8b24d0ed7b9fc669f6826c5bc3eb7412fc765691f59b83ae195b2/typed_ast-1.5.5-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:61443214d9b4c660dcf4b5307f15c12cb30bdfe9588ce6158f4a005baeb167b2", size = 860314, upload-time = "2023-07-04T18:37:18.215Z" }, - { url = "https://files.pythonhosted.org/packages/d9/3c/4af750e6c673a0dd6c7b9f5b5e5ed58ec51a2e4e744081781c664d369dfa/typed_ast-1.5.5-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:6eb936d107e4d474940469e8ec5b380c9b329b5f08b78282d46baeebd3692dc9", size = 898108, upload-time = "2023-07-04T18:37:20.095Z" }, - { url = "https://files.pythonhosted.org/packages/03/8d/d0a4d1e060e1e8dda2408131a0cc7633fc4bc99fca5941dcb86c461dfe01/typed_ast-1.5.5-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:e48bf27022897577d8479eaed64701ecaf0467182448bd95759883300ca818c8", size = 881971, upload-time = "2023-07-04T18:37:21.912Z" }, - { url = "https://files.pythonhosted.org/packages/90/83/f28d2c912cd010a09b3677ac69d23181045eb17e358914ab739b7fdee530/typed_ast-1.5.5-cp311-cp311-win_amd64.whl", hash = "sha256:83509f9324011c9a39faaef0922c6f720f9623afe3fe220b6d0b15638247206b", size = 139286, upload-time = "2023-07-04T18:37:23.625Z" }, -] - -[[package]] -name = "typing-extensions" -version = "4.16.0" +version = "2.5.0" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/f6/cc/6253133b5bb138fc3306cebfbda2c520f545d36b5be2c7255cc528bb45d6/typing_extensions-4.16.0.tar.gz", hash = "sha256:dc983d19a509c94dba722ee6abd33940f7c05a89e243c47e907eb4db6f1a43e5", size = 113555, upload-time = "2026-07-02T08:40:05.92Z" } +sdist = { url = "https://files.pythonhosted.org/packages/37/72/88311445fd44c455c7d553e61f95412cf89054308a1aa2434ab835075fc5/termcolor-2.5.0.tar.gz", hash = "sha256:998d8d27da6d48442e8e1f016119076b690d962507531df4890fcd2db2ef8a6f", size = 13057, upload-time = "2024-10-06T19:50:04.115Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/49/d3/b8441a820a491ddfc024b0b0cf0393375b75ea13866d9c66727e54c2fc80/typing_extensions-4.16.0-py3-none-any.whl", hash = "sha256:481caa481374e813c1b176ada14e97f1f67a4539ce9cfeb3f350d78d6370c2e8", size = 45571, upload-time = "2026-07-02T08:40:04.659Z" }, + { url = "https://files.pythonhosted.org/packages/7f/be/df630c387a0a054815d60be6a97eb4e8f17385d5d6fe660e1c02750062b4/termcolor-2.5.0-py3-none-any.whl", hash = "sha256:37b17b5fc1e604945c2642c872a3764b5d547a48009871aea3edd3afa180afb8", size = 7755, upload-time = "2024-10-06T19:50:02.097Z" }, ] From c7c6b391d1bc5e74d7f01262a0e229b0d5a35b61 Mon Sep 17 00:00:00 2001 From: jpptm Date: Sun, 13 Sep 2026 12:01:14 +1000 Subject: [PATCH 2/2] Format the codebase with ./b format MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Pure mechanical output of `./b format --all` using the configs added in the previous commit. No behavioural change: the sim builds and all six unit tests pass, and `./b format --check --all` is clean. Most of the diff is NamespaceIndentation: All, which NUbots' .clang-format sets and this codebase was not written with. mujoco/idl_gen/ is untouched — it is generated, and excluded in tools/format.py. Co-Authored-By: Claude Opus 5 --- mujoco/CMakeLists.txt | 71 +- mujoco/b.py | 2 +- mujoco/cmake/FetchGlfw.cmake | 39 +- mujoco/cmake/FetchJson.cmake | 10 +- mujoco/cmake/FetchNUClear.cmake | 37 +- mujoco/cmake/K1SimRole.cmake | 53 +- mujoco/cmake/MuJoCoTarget.cmake | 45 +- mujoco/cmake/generate_role.py | 71 +- mujoco/module/Camera/CMakeLists.txt | 30 +- mujoco/module/Camera/src/Camera.cpp | 482 ++++---- mujoco/module/Camera/src/Camera.hpp | 70 +- mujoco/module/Camera/src/CameraConfig.hpp | 70 +- mujoco/module/Camera/src/EglContext.hpp | 148 +-- .../module/Camera/src/SharedImageWriter.cpp | 122 +- .../module/Camera/src/SharedImageWriter.hpp | 150 +-- mujoco/module/Camera/src/SharedPoseWriter.cpp | 54 +- mujoco/module/Camera/src/SharedPoseWriter.hpp | 84 +- mujoco/module/ConsoleLog/src/ConsoleLog.cpp | 20 +- mujoco/module/ConsoleLog/src/ConsoleLog.hpp | 12 +- mujoco/module/Locomotion/CMakeLists.txt | 6 +- mujoco/module/Locomotion/src/LocoMath.hpp | 50 +- mujoco/module/Locomotion/src/Locomotion.cpp | 103 +- mujoco/module/Locomotion/src/Locomotion.hpp | 42 +- .../Locomotion/src/LocomotionController.cpp | 448 +++---- .../Locomotion/src/LocomotionController.hpp | 212 ++-- mujoco/module/SdkBridge/CMakeLists.txt | 41 +- .../module/SdkBridge/src/DdsParticipant.cpp | 102 +- .../module/SdkBridge/src/DdsParticipant.hpp | 84 +- mujoco/module/SdkBridge/src/RpcDispatch.cpp | 196 +-- mujoco/module/SdkBridge/src/RpcDispatch.hpp | 76 +- mujoco/module/SdkBridge/src/RpcServer.cpp | 201 +-- mujoco/module/SdkBridge/src/RpcServer.hpp | 46 +- mujoco/module/SdkBridge/src/SdkBridge.cpp | 97 +- mujoco/module/SdkBridge/src/SdkBridge.hpp | 28 +- .../module/SdkBridge/src/StatePublisher.cpp | 240 ++-- .../module/SdkBridge/src/StatePublisher.hpp | 54 +- .../SdkBridge/test_support/CMakeLists.txt | 11 +- .../SdkBridge/test_support/SyntheticState.cpp | 188 +-- .../SdkBridge/test_support/SyntheticState.hpp | 26 +- mujoco/module/SdkBridge/test_support/main.cpp | 8 +- mujoco/module/Simulation/CMakeLists.txt | 2 +- mujoco/module/Simulation/src/SimCore.cpp | 1097 +++++++++-------- mujoco/module/Simulation/src/SimCore.hpp | 298 ++--- mujoco/module/Simulation/src/Simulation.cpp | 218 ++-- mujoco/module/Simulation/src/Simulation.hpp | 30 +- mujoco/module/Supervisor/CMakeLists.txt | 7 +- .../Supervisor/src/GameControllerPacket.hpp | 278 ++--- mujoco/module/Supervisor/src/Supervisor.cpp | 177 +-- mujoco/module/Supervisor/src/Supervisor.hpp | 60 +- .../Supervisor/src/SupervisorConfig.hpp | 182 +-- .../module/Supervisor/src/SupervisorLogic.hpp | 318 ++--- .../Supervisor/src/SupervisorPlacement.hpp | 121 +- mujoco/module/Viewer/CMakeLists.txt | 6 +- mujoco/module/Viewer/src/Viewer.cpp | 639 +++++----- mujoco/module/Viewer/src/Viewer.hpp | 16 +- mujoco/roles/sim/soccer.role | 18 +- mujoco/shared/CliOptions.hpp | 133 +- mujoco/shared/gl/XThreads.hpp | 16 +- mujoco/shared/k1/BoosterApi.hpp | 88 +- mujoco/shared/k1/JointIndex.hpp | 94 +- mujoco/shared/message/Commands.hpp | 92 +- mujoco/shared/message/SimMessages.hpp | 92 +- mujoco/shared/sim/HeadPose.hpp | 80 +- mujoco/shared/sim/ModelMap.hpp | 96 +- mujoco/shared/sim/PdController.hpp | 54 +- mujoco/shared/sim/StepController.hpp | 28 +- mujoco/shared/util/Config.hpp | 66 +- mujoco/src/main.cpp | 8 +- mujoco/test/contract/check_model.py | 18 +- .../contract/host_client/sdk_client_check.cpp | 135 +- mujoco/test/contract/test_sdk_roundtrip.py | 37 +- mujoco/test/unit/CMakeLists.txt | 25 +- mujoco/test/unit/test_locomotion.cpp | 518 ++++---- mujoco/test/unit/test_model_load.cpp | 22 +- mujoco/test/unit/test_pd_stand.cpp | 74 +- mujoco/test/unit/test_rpc_dispatch.cpp | 26 +- mujoco/test/unit/test_supervisor.cpp | 628 +++++----- mujoco/tools/_util.py | 1 + mujoco/tools/build.py | 1 + mujoco/tools/configure.py | 17 +- mujoco/tools/format.py | 1 + mujoco/tools/image.py | 1 + mujoco/tools/roles.py | 3 +- mujoco/tools/run.py | 1 + mujoco/tools/walk_surface_sweep.py | 17 +- 85 files changed, 4850 insertions(+), 4818 deletions(-) diff --git a/mujoco/CMakeLists.txt b/mujoco/CMakeLists.txt index d543a9c..65d480d 100644 --- a/mujoco/CMakeLists.txt +++ b/mujoco/CMakeLists.txt @@ -1,8 +1,12 @@ cmake_minimum_required(VERSION 3.22) -project(k1_mujoco_sim VERSION 0.1.0 LANGUAGES C CXX) +project( + k1_mujoco_sim + VERSION 0.1.0 + LANGUAGES C CXX +) -# Self-contained MuJoCo replacement for the Webots+mck K1 simulation. -# Prerequisite: tools/install_deps.sh (installs MuJoCo + Fast-DDS into .deps/install). +# Self-contained MuJoCo replacement for the Webots+mck K1 simulation. Prerequisite: tools/install_deps.sh (installs +# MuJoCo + Fast-DDS into .deps/install). set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -10,14 +14,16 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release CACHE STRING "" FORCE) + set(CMAKE_BUILD_TYPE + Release + CACHE STRING "" FORCE + ) endif() - -# Dependency prefix: the docker image (docker/Dockerfile) bakes deps into -# /opt/k1sim-deps; a native fallback build (tools/install_deps.sh) uses .deps/install. +# Dependency prefix: the docker image (docker/Dockerfile) bakes deps into /opt/k1sim-deps; a native fallback build +# (tools/install_deps.sh) uses .deps/install. if(DEFINED ENV{K1SIM_DEPS_PREFIX}) - list(APPEND CMAKE_PREFIX_PATH "$ENV{K1SIM_DEPS_PREFIX}") + list(APPEND CMAKE_PREFIX_PATH "$ENV{K1SIM_DEPS_PREFIX}") endif() list(APPEND CMAKE_PREFIX_PATH "${CMAKE_CURRENT_SOURCE_DIR}/.deps/install" "/opt/k1sim-deps") @@ -31,16 +37,16 @@ find_package(yaml-cpp REQUIRED) find_package(fastcdr REQUIRED) find_package(fastrtps REQUIRED) # Fast-DDS 2.x exports the 'fastrtps' package -# Single include root: every include is written relative to mujoco/ -# (e.g. "shared/k1/JointIndex.hpp", "module/Simulation/src/Simulation.hpp"). +# Single include root: every include is written relative to mujoco/ (e.g. "shared/k1/JointIndex.hpp", +# "module/Simulation/src/Simulation.hpp"). add_library(k1sim_shared INTERFACE) target_include_directories(k1sim_shared INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(k1sim_shared INTERFACE mujoco::mujoco NUClear::nuclear yaml-cpp Threads::Threads) -# XInitThreads() (shared/gl/XThreads.hpp) is called by every role's main to make -# Xlib thread-safe for the Viewer + Camera GL threads; link X11 for the symbol. +# XInitThreads() (shared/gl/XThreads.hpp) is called by every role's main to make Xlib thread-safe for the Viewer + +# Camera GL threads; link X11 for the symbol. find_package(X11) if(X11_FOUND) - target_link_libraries(k1sim_shared INTERFACE ${X11_LIBRARIES}) + target_link_libraries(k1sim_shared INTERFACE ${X11_LIBRARIES}) endif() target_compile_definitions(k1sim_shared INTERFACE K1SIM_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") @@ -57,32 +63,27 @@ set(K1SIM_ROLE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/cmake) include(cmake/K1SimRole.cmake) file(GLOB_RECURSE role_files CONFIGURE_DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/roles/*.role") foreach(role_file ${role_files}) - file(RELATIVE_PATH rel "${CMAKE_CURRENT_SOURCE_DIR}/roles" "${role_file}") - get_filename_component(role_name "${rel}" NAME_WE) - get_filename_component(role_path "${rel}" DIRECTORY) - if(role_path) - string(REPLACE "/" "-" role_prefix "${role_path}") - set(role "${role_prefix}-${role_name}") - else() - set(role "${role_name}") - endif() - option("ROLE_${role}" "Build the ${role} role" ON) - if(ROLE_${role}) - include("${role_file}") - endif() + file(RELATIVE_PATH rel "${CMAKE_CURRENT_SOURCE_DIR}/roles" "${role_file}") + get_filename_component(role_name "${rel}" NAME_WE) + get_filename_component(role_path "${rel}" DIRECTORY) + if(role_path) + string(REPLACE "/" "-" role_prefix "${role_path}") + set(role "${role_prefix}-${role_name}") + else() + set(role "${role_name}") + endif() + option("ROLE_${role}" "Build the ${role} role" ON) + if(ROLE_${role}) + include("${role_file}") + endif() endforeach() -# Legacy single-binary entry (kept during the roles transition; exercised by the -# contract test). Equivalent to the sim/soccer role minus Camera/Supervisor. +# Legacy single-binary entry (kept during the roles transition; exercised by the contract test). Equivalent to the +# sim/soccer role minus Camera/Supervisor. add_executable(k1_mujoco_sim src/main.cpp) target_link_libraries( - k1_mujoco_sim - PRIVATE k1sim_module_consolelog - k1sim_module_simulation - k1sim_module_sdkbridge - k1sim_module_locomotion - k1sim_module_viewer - k1sim_shared + k1_mujoco_sim PRIVATE k1sim_module_consolelog k1sim_module_simulation k1sim_module_sdkbridge k1sim_module_locomotion + k1sim_module_viewer k1sim_shared ) enable_testing() diff --git a/mujoco/b.py b/mujoco/b.py index 198c53f..cc7c81b 100755 --- a/mujoco/b.py +++ b/mujoco/b.py @@ -58,7 +58,7 @@ def main(): sys.exit(0 if not argv else 1) module = _load(candidates[match]) - rest = argv[len(match):] + rest = argv[len(match) :] parser = argparse.ArgumentParser(prog="./b " + " ".join(match)) if hasattr(module, "register"): module.register(parser) diff --git a/mujoco/cmake/FetchGlfw.cmake b/mujoco/cmake/FetchGlfw.cmake index 2edba4b..f309079 100644 --- a/mujoco/cmake/FetchGlfw.cmake +++ b/mujoco/cmake/FetchGlfw.cmake @@ -1,17 +1,32 @@ # GLFW for the interactive viewer window (MuJoCo mjr rendering is OpenGL). include(FetchContent) -set(GLFW_BUILD_EXAMPLES OFF CACHE BOOL "" FORCE) -set(GLFW_BUILD_TESTS OFF CACHE BOOL "" FORCE) -set(GLFW_BUILD_DOCS OFF CACHE BOOL "" FORCE) -set(GLFW_INSTALL OFF CACHE BOOL "" FORCE) -# X11 only: the docker run path passes the X11 socket through, and Wayland -# support drags in pkg-config/wayland-scanner at configure time. -set(GLFW_BUILD_WAYLAND OFF CACHE BOOL "" FORCE) -set(GLFW_BUILD_X11 ON CACHE BOOL "" FORCE) - -FetchContent_Declare( - glfw - URL https://github.com/glfw/glfw/archive/refs/tags/3.4.tar.gz +set(GLFW_BUILD_EXAMPLES + OFF + CACHE BOOL "" FORCE +) +set(GLFW_BUILD_TESTS + OFF + CACHE BOOL "" FORCE +) +set(GLFW_BUILD_DOCS + OFF + CACHE BOOL "" FORCE +) +set(GLFW_INSTALL + OFF + CACHE BOOL "" FORCE ) +# X11 only: the docker run path passes the X11 socket through, and Wayland support drags in pkg-config/wayland-scanner +# at configure time. +set(GLFW_BUILD_WAYLAND + OFF + CACHE BOOL "" FORCE +) +set(GLFW_BUILD_X11 + ON + CACHE BOOL "" FORCE +) + +FetchContent_Declare(glfw URL https://github.com/glfw/glfw/archive/refs/tags/3.4.tar.gz) FetchContent_MakeAvailable(glfw) diff --git a/mujoco/cmake/FetchJson.cmake b/mujoco/cmake/FetchJson.cmake index cad31f9..c0d3578 100644 --- a/mujoco/cmake/FetchJson.cmake +++ b/mujoco/cmake/FetchJson.cmake @@ -1,10 +1,10 @@ # nlohmann/json (header-only) — used for the Booster RPC JSON bodies. include(FetchContent) -set(JSON_BuildTests OFF CACHE BOOL "" FORCE) - -FetchContent_Declare( - nlohmann_json - URL https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz +set(JSON_BuildTests + OFF + CACHE BOOL "" FORCE ) + +FetchContent_Declare(nlohmann_json URL https://github.com/nlohmann/json/archive/refs/tags/v3.11.3.tar.gz) FetchContent_MakeAvailable(nlohmann_json) diff --git a/mujoco/cmake/FetchNUClear.cmake b/mujoco/cmake/FetchNUClear.cmake index 49c0767..7df7243 100644 --- a/mujoco/cmake/FetchNUClear.cmake +++ b/mujoco/cmake/FetchNUClear.cmake @@ -1,28 +1,29 @@ -# NUClear, pinned to the same commit NUbots_K1's docker image installs -# (docker/Dockerfile "install-from-source .../NUClear/archive/925dca0f..."). +# NUClear, pinned to the same commit NUbots_K1's docker image installs (docker/Dockerfile "install-from-source +# .../NUClear/archive/925dca0f..."). include(FetchContent) -set(BUILD_TESTS OFF CACHE BOOL "" FORCE) +set(BUILD_TESTS + OFF + CACHE BOOL "" FORCE +) FetchContent_Declare( - NUClear - URL https://github.com/Fastcode/NUClear/archive/925dca0f31484a7df64fd335de5a6c9335483c7f.tar.gz + NUClear URL https://github.com/Fastcode/NUClear/archive/925dca0f31484a7df64fd335de5a6c9335483c7f.tar.gz ) -# NUClear's top-level CMakeLists resolves its helper modules (ClangTidy, -# CompilerOptions, Sanitizers) via CMAKE_SOURCE_DIR, which under FetchContent is -# *this* project — so add its module dirs to our path before add_subdirectory. +# NUClear's top-level CMakeLists resolves its helper modules (ClangTidy, CompilerOptions, Sanitizers) via +# CMAKE_SOURCE_DIR, which under FetchContent is *this* project — so add its module dirs to our path before +# add_subdirectory. FetchContent_GetProperties(NUClear) if(NOT nuclear_POPULATED) - FetchContent_Populate(NUClear) - list(APPEND CMAKE_MODULE_PATH "${nuclear_SOURCE_DIR}/cmake" "${nuclear_SOURCE_DIR}/cmake/Modules") - add_subdirectory("${nuclear_SOURCE_DIR}" "${nuclear_BINARY_DIR}") + FetchContent_Populate(NUClear) + list(APPEND CMAKE_MODULE_PATH "${nuclear_SOURCE_DIR}/cmake" "${nuclear_SOURCE_DIR}/cmake/Modules") + add_subdirectory("${nuclear_SOURCE_DIR}" "${nuclear_BINARY_DIR}") - # NUClear only exports include dirs through its install rules; for build-tree - # (FetchContent) consumers, expose the generated umbrella header - # (/include/nuclear) and the source headers it references. - target_include_directories( - nuclear SYSTEM PUBLIC "$" - "$" - ) + # NUClear only exports include dirs through its install rules; for build-tree (FetchContent) consumers, expose the + # generated umbrella header (/include/nuclear) and the source headers it references. + target_include_directories( + nuclear SYSTEM PUBLIC "$" + "$" + ) endif() diff --git a/mujoco/cmake/K1SimRole.cmake b/mujoco/cmake/K1SimRole.cmake index 351e2a5..7e01d4a 100644 --- a/mujoco/cmake/K1SimRole.cmake +++ b/mujoco/cmake/K1SimRole.cmake @@ -1,38 +1,35 @@ # Role → binary machinery (a lighter port of NUbots' nuclear_role()). # -# A mujoco/roles/**/*.role file is a CMake snippet that calls k1sim_role() with a -# list of module short-names (e.g. `Simulation SdkBridge Locomotion`). Each name -# X maps to: the reactor class k1sim::module::X, the header module/X/src/X.hpp, -# and the static library target k1sim_module_. generate_role.py emits -# a .cpp main() that installs ChronoController + each listed module. +# A mujoco/roles/**/*.role file is a CMake snippet that calls k1sim_role() with a list of module short-names (e.g. +# `Simulation SdkBridge Locomotion`). Each name X maps to: the reactor class k1sim::module::X, the header +# module/X/src/X.hpp, and the static library target k1sim_module_. generate_role.py emits a .cpp +# main() that installs ChronoController + each listed module. # -# The enclosing glob loop (in CMakeLists.txt) sets `role` (dashed target name, e.g. -# sim-soccer), `role_name` (soccer) and `role_path` (sim) before include()-ing the -# role file, so the binary lands at bin//. +# The enclosing glob loop (in CMakeLists.txt) sets `role` (dashed target name, e.g. sim-soccer), `role_name` (soccer) +# and `role_path` (sim) before include()-ing the role file, so the binary lands at bin//. find_package(Python3 REQUIRED) function(k1sim_role) - set(role_modules ${ARGN}) + set(role_modules ${ARGN}) - add_custom_command( - OUTPUT "${role}.cpp" - COMMAND ${Python3_EXECUTABLE} "${K1SIM_ROLE_DIR}/generate_role.py" "${role}.cpp" - "${PROJECT_SOURCE_DIR}" ${role_modules} - DEPENDS "${K1SIM_ROLE_DIR}/generate_role.py" - COMMENT "Generating role ${role}" - VERBATIM - ) + add_custom_command( + OUTPUT "${role}.cpp" + COMMAND ${Python3_EXECUTABLE} "${K1SIM_ROLE_DIR}/generate_role.py" "${role}.cpp" "${PROJECT_SOURCE_DIR}" + ${role_modules} + DEPENDS "${K1SIM_ROLE_DIR}/generate_role.py" + COMMENT "Generating role ${role}" + VERBATIM + ) - add_executable(${role} "${role}.cpp") - set_target_properties( - ${role} PROPERTIES OUTPUT_NAME "${role_name}" - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin/${role_path}" - ) + add_executable(${role} "${role}.cpp") + set_target_properties( + ${role} PROPERTIES OUTPUT_NAME "${role_name}" RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin/${role_path}" + ) - set(role_libs "") - foreach(m ${role_modules}) - string(TOLOWER "${m}" m_lc) - list(APPEND role_libs "k1sim_module_${m_lc}") - endforeach() - target_link_libraries(${role} PRIVATE ${role_libs} k1sim_shared) + set(role_libs "") + foreach(m ${role_modules}) + string(TOLOWER "${m}" m_lc) + list(APPEND role_libs "k1sim_module_${m_lc}") + endforeach() + target_link_libraries(${role} PRIVATE ${role_libs} k1sim_shared) endfunction() diff --git a/mujoco/cmake/MuJoCoTarget.cmake b/mujoco/cmake/MuJoCoTarget.cmake index 8c8155d..4e52797 100644 --- a/mujoco/cmake/MuJoCoTarget.cmake +++ b/mujoco/cmake/MuJoCoTarget.cmake @@ -1,50 +1,49 @@ -# Imported target for the prebuilt MuJoCo installed by tools/install_deps.sh. -# Override the location with -DMUJOCO_DIR=... or the MUJOCO_DIR environment variable. +# Imported target for the prebuilt MuJoCo installed by tools/install_deps.sh. Override the location with +# -DMUJOCO_DIR=... or the MUJOCO_DIR environment variable. set(MUJOCO_VERSION 3.10.0) set(_mj_hints "") if(MUJOCO_DIR) - list(APPEND _mj_hints "${MUJOCO_DIR}") + list(APPEND _mj_hints "${MUJOCO_DIR}") endif() if(DEFINED ENV{MUJOCO_DIR}) - list(APPEND _mj_hints "$ENV{MUJOCO_DIR}") + list(APPEND _mj_hints "$ENV{MUJOCO_DIR}") endif() if(DEFINED ENV{K1SIM_DEPS_PREFIX}) - list(APPEND _mj_hints "$ENV{K1SIM_DEPS_PREFIX}/mujoco-${MUJOCO_VERSION}") + list(APPEND _mj_hints "$ENV{K1SIM_DEPS_PREFIX}/mujoco-${MUJOCO_VERSION}") endif() list(APPEND _mj_hints "${CMAKE_CURRENT_SOURCE_DIR}/.deps/install/mujoco-${MUJOCO_VERSION}" - "/opt/k1sim-deps/mujoco-${MUJOCO_VERSION}") + "/opt/k1sim-deps/mujoco-${MUJOCO_VERSION}" +) set(_mj_includes "") set(_mj_libs "") foreach(root ${_mj_hints}) - list(APPEND _mj_includes "${root}/include") - list(APPEND _mj_libs "${root}/lib") + list(APPEND _mj_includes "${root}/include") + list(APPEND _mj_libs "${root}/lib") endforeach() find_path( - MUJOCO_INCLUDE_DIR mujoco/mujoco.h - HINTS ${_mj_includes} - NO_DEFAULT_PATH + MUJOCO_INCLUDE_DIR mujoco/mujoco.h + HINTS ${_mj_includes} + NO_DEFAULT_PATH ) find_library( - MUJOCO_LIBRARY mujoco - HINTS ${_mj_libs} - NO_DEFAULT_PATH + MUJOCO_LIBRARY mujoco + HINTS ${_mj_libs} + NO_DEFAULT_PATH ) if(NOT MUJOCO_INCLUDE_DIR OR NOT MUJOCO_LIBRARY) - message( - FATAL_ERROR - "MuJoCo ${MUJOCO_VERSION} not found (searched: ${_mj_hints}).\n" - "Build inside docker (docker/k1sim.sh build), run tools/install_deps.sh, " - "or point MUJOCO_DIR at a MuJoCo install." - ) + message( + FATAL_ERROR + "MuJoCo ${MUJOCO_VERSION} not found (searched: ${_mj_hints}).\n" + "Build inside docker (docker/k1sim.sh build), run tools/install_deps.sh, " + "or point MUJOCO_DIR at a MuJoCo install." + ) endif() add_library(mujoco::mujoco SHARED IMPORTED) set_target_properties( - mujoco::mujoco - PROPERTIES IMPORTED_LOCATION "${MUJOCO_LIBRARY}" - INTERFACE_INCLUDE_DIRECTORIES "${MUJOCO_INCLUDE_DIR}" + mujoco::mujoco PROPERTIES IMPORTED_LOCATION "${MUJOCO_LIBRARY}" INTERFACE_INCLUDE_DIRECTORIES "${MUJOCO_INCLUDE_DIR}" ) diff --git a/mujoco/cmake/generate_role.py b/mujoco/cmake/generate_role.py index 2a48c34..0a733c8 100755 --- a/mujoco/cmake/generate_role.py +++ b/mujoco/cmake/generate_role.py @@ -15,47 +15,52 @@ source_root = sys.argv[2] modules = sys.argv[3:] -lines = ['#include ', '#include ', '', '#include "shared/CliOptions.hpp"', - '#include "shared/gl/XThreads.hpp"'] +lines = [ + "#include ", + "#include ", + "", + '#include "shared/CliOptions.hpp"', + '#include "shared/gl/XThreads.hpp"', +] for m in modules: - header = f'module/{m}/src/{m}.hpp' + header = f"module/{m}/src/{m}.hpp" if not os.path.isfile(os.path.join(source_root, header)): - raise SystemExit(f'generate_role.py: cannot find header {header} for module {m}') + raise SystemExit(f"generate_role.py: cannot find header {header} for module {m}") lines.append(f'#include "{header}"') lines += [ - '', - 'namespace {', - 'void handle_signal(int /*signum*/) {', - ' if (NUClear::PowerPlant::powerplant != nullptr) {', - ' NUClear::PowerPlant::powerplant->shutdown();', - ' }', - '}', - '} // namespace', - '', - 'int main(int argc, char** argv) {', - ' k1sim::init_x_threads(); // must precede any GL/X11 use (Viewer + Camera render threads)', - ' k1sim::cli() = k1sim::parse_cli(argc, argv);', - '', - ' NUClear::Configuration config;', - ' config.default_pool_concurrency = 4;', - ' NUClear::PowerPlant plant(config);', - '', - ' plant.install();', + "", + "namespace {", + "void handle_signal(int /*signum*/) {", + " if (NUClear::PowerPlant::powerplant != nullptr) {", + " NUClear::PowerPlant::powerplant->shutdown();", + " }", + "}", + "} // namespace", + "", + "int main(int argc, char** argv) {", + " k1sim::init_x_threads(); // must precede any GL/X11 use (Viewer + Camera render threads)", + " k1sim::cli() = k1sim::parse_cli(argc, argv);", + "", + " NUClear::Configuration config;", + " config.default_pool_concurrency = 4;", + " NUClear::PowerPlant plant(config);", + "", + " plant.install();", ] for m in modules: - lines.append(f' plant.install();') + lines.append(f" plant.install();") lines += [ - '', - ' std::signal(SIGINT, handle_signal);', - ' std::signal(SIGTERM, handle_signal);', - '', - ' plant.start();', - ' return 0;', - '}', - '', + "", + " std::signal(SIGINT, handle_signal);", + " std::signal(SIGTERM, handle_signal);", + "", + " plant.start();", + " return 0;", + "}", + "", ] -with open(out_path, 'w', encoding='utf-8') as f: - f.write('\n'.join(lines)) +with open(out_path, "w", encoding="utf-8") as f: + f.write("\n".join(lines)) diff --git a/mujoco/module/Camera/CMakeLists.txt b/mujoco/module/Camera/CMakeLists.txt index fd81b5d..e22ab26 100644 --- a/mujoco/module/Camera/CMakeLists.txt +++ b/mujoco/module/Camera/CMakeLists.txt @@ -1,23 +1,17 @@ -# Boost.Interprocess (header-only) needs the Boost headers themselves and -# librt/pthread. NOTE FOR THE LEAD: the k1sim:latest image does not currently -# have Boost headers installed (verified: /usr/include/boost/interprocess is -# absent) -- add `libboost-dev` to mujoco/docker/Dockerfile's apt-get install -# list (matches the package NUbots_K1's own docker/Dockerfile installs for the -# same header-only usage). pthread already flows in transitively via -# k1sim_shared (Threads::Threads); librt's symbols are provided by libc itself -# on glibc >= 2.34 (Ubuntu 22.04), but -lrt is kept for portability/older glibc. +# Boost.Interprocess (header-only) needs the Boost headers themselves and librt/pthread. NOTE FOR THE LEAD: the +# k1sim:latest image does not currently have Boost headers installed (verified: /usr/include/boost/interprocess is +# absent) -- add `libboost-dev` to mujoco/docker/Dockerfile's apt-get install list (matches the package NUbots_K1's own +# docker/Dockerfile installs for the same header-only usage). pthread already flows in transitively via k1sim_shared +# (Threads::Threads); librt's symbols are provided by libc itself on glibc >= 2.34 (Ubuntu 22.04), but -lrt is kept for +# portability/older glibc. find_package(Boost REQUIRED) -# EGL (offscreen GL, no GLFW) — decouples the camera render thread from the -# Viewer's GLFW/Xlib state and works headless. libegl1-mesa-dev is in the image. +# EGL (offscreen GL, no GLFW) — decouples the camera render thread from the Viewer's GLFW/Xlib state and works headless. +# libegl1-mesa-dev is in the image. find_package(OpenGL REQUIRED COMPONENTS EGL) -add_library(k1sim_module_camera STATIC - src/Camera.cpp - src/SharedImageWriter.cpp - src/SharedPoseWriter.cpp -) +add_library(k1sim_module_camera STATIC src/Camera.cpp src/SharedImageWriter.cpp src/SharedPoseWriter.cpp) target_link_libraries( - k1sim_module_camera - PUBLIC k1sim_shared - PRIVATE Boost::headers OpenGL::EGL OpenGL::GL rt + k1sim_module_camera + PUBLIC k1sim_shared + PRIVATE Boost::headers OpenGL::EGL OpenGL::GL rt ) diff --git a/mujoco/module/Camera/src/Camera.cpp b/mujoco/module/Camera/src/Camera.cpp index 79f86ea..8ca6e22 100644 --- a/mujoco/module/Camera/src/Camera.cpp +++ b/mujoco/module/Camera/src/Camera.cpp @@ -1,12 +1,11 @@ #include "module/Camera/src/Camera.hpp" -#include - #include #include #include #include #include +#include #include #include #include @@ -20,260 +19,263 @@ namespace k1sim::module { -namespace { - -// Mirrors K1Camera.cpp's MAX_IMAGE_BYTES exactly -- the reader drops (logs WARN -// and skips) any frame whose data_size exceeds this, so it's not enough to just -// fit in the segment; we must fit in what the reader will accept. -constexpr std::size_t kMaxImageBytes = 2 * 1024 * 1024; - -// Avoid M_PI: not reliably available under this project's -std=c++17 build (see -// test/unit/test_supervisor.cpp's identical workaround/comment). -constexpr double kPi = 3.14159265358979323846; - -} // namespace - -Camera::Camera(std::unique_ptr environment) : Reactor(std::move(environment)) { - - on().then([this] { - camera::CameraConfig cfg = camera::load_config(config::load("camera.yaml")); - log("Camera: starting offscreen render thread (segment", - cfg.segment.c_str(), - ",", - cfg.width, - "x", - cfg.height, - "@", - cfg.fps, - "fps, mjcf camera", - cfg.mjcf_camera.c_str(), - ")"); - running_.store(true, std::memory_order_release); - render_thread_ = std::thread(&Camera::render_loop, this, cfg); - }); - - // Deliberately NOT MainThread: this reaction only stores plain pointers (no - // GL work), so it can run on any NUClear pool thread. render_thread_ is the - // only thread that ever touches GL/mjv/mjr state or handles_ after startup. - on>().then([this](const message::SimHandles& h) { - handles_ = h; - handles_ready_.store(true, std::memory_order_release); - }); - - on().then([this] { - running_.store(false, std::memory_order_release); - if (render_thread_.joinable()) { - render_thread_.join(); - } - log("Camera shutting down"); - }); -} - -void Camera::render_loop(camera::CameraConfig cfg) { - // Everything below (GLFW window, mjvScene/mjrContext, the shared-memory - // writer) is local to this function/thread on purpose -- see the - // class-level comment in Camera.hpp for the threading rationale. - - const auto frame_bytes = static_cast(cfg.width) * static_cast(cfg.height) * 3; - if (frame_bytes > kMaxImageBytes) { - log("Camera: configured", - cfg.width, - "x", - cfg.height, - "rgb8 exceeds K1Camera's MAX_IMAGE_BYTES (2 MiB) -- the reader would drop " - "every frame. Lower width/height in config/camera.yaml."); - return; - } - - // Offscreen OpenGL via EGL (not GLFW): no shared GLFW/Xlib global state with - // module::Viewer's on-screen window, so the two render threads don't corrupt - // each other's heap; also works with no display (headless). RAII — tears the - // context down on any return below. - camera::EglContext egl(cfg.width, cfg.height); - if (!egl.valid()) { - log("Camera: EGL offscreen context creation failed -- no camera frames " - "will be published (need libEGL + a GPU/mesa device)"); - return; - } - - // Created from config alone (doesn't need the model), so the segment exists - // whether the sim or NUbots' K1Camera process starts first -- K1Camera - // retries opening it every 500 ms until it appears. - std::unique_ptr writer; - try { - writer = std::make_unique(cfg.segment, cfg.width, cfg.height); - } - catch (const std::exception& e) { - log("Camera: failed to create shared-memory segment", - cfg.segment.c_str(), - ":", - e.what()); - return; + namespace { + + // Mirrors K1Camera.cpp's MAX_IMAGE_BYTES exactly -- the reader drops (logs WARN + // and skips) any frame whose data_size exceeds this, so it's not enough to just + // fit in the segment; we must fit in what the reader will accept. + constexpr std::size_t kMaxImageBytes = 2 * 1024 * 1024; + + // Avoid M_PI: not reliably available under this project's -std=c++17 build (see + // test/unit/test_supervisor.cpp's identical workaround/comment). + constexpr double kPi = 3.14159265358979323846; + + } // namespace + + Camera::Camera(std::unique_ptr environment) : Reactor(std::move(environment)) { + + on().then([this] { + camera::CameraConfig cfg = camera::load_config(config::load("camera.yaml")); + log("Camera: starting offscreen render thread (segment", + cfg.segment.c_str(), + ",", + cfg.width, + "x", + cfg.height, + "@", + cfg.fps, + "fps, mjcf camera", + cfg.mjcf_camera.c_str(), + ")"); + running_.store(true, std::memory_order_release); + render_thread_ = std::thread(&Camera::render_loop, this, cfg); + }); + + // Deliberately NOT MainThread: this reaction only stores plain pointers (no + // GL work), so it can run on any NUClear pool thread. render_thread_ is the + // only thread that ever touches GL/mjv/mjr state or handles_ after startup. + on>().then([this](const message::SimHandles& h) { + handles_ = h; + handles_ready_.store(true, std::memory_order_release); + }); + + on().then([this] { + running_.store(false, std::memory_order_release); + if (render_thread_.joinable()) { + render_thread_.join(); + } + log("Camera shutting down"); + }); } - while (running_.load(std::memory_order_acquire) && !handles_ready_.load(std::memory_order_acquire)) { - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - } - if (!running_.load(std::memory_order_acquire)) { - return; - } + void Camera::render_loop(camera::CameraConfig cfg) { + // Everything below (GLFW window, mjvScene/mjrContext, the shared-memory + // writer) is local to this function/thread on purpose -- see the + // class-level comment in Camera.hpp for the threading rationale. + + const auto frame_bytes = static_cast(cfg.width) * static_cast(cfg.height) * 3; + if (frame_bytes > kMaxImageBytes) { + log("Camera: configured", + cfg.width, + "x", + cfg.height, + "rgb8 exceeds K1Camera's MAX_IMAGE_BYTES (2 MiB) -- the reader would drop " + "every frame. Lower width/height in config/camera.yaml."); + return; + } - const mjModel* model = handles_.model; - mjData* data = handles_.data; - std::mutex* sim_mutex = handles_.mutex; + // Offscreen OpenGL via EGL (not GLFW): no shared GLFW/Xlib global state with + // module::Viewer's on-screen window, so the two render threads don't corrupt + // each other's heap; also works with no display (headless). RAII — tears the + // context down on any return below. + camera::EglContext egl(cfg.width, cfg.height); + if (!egl.valid()) { + log( + "Camera: EGL offscreen context creation failed -- no camera frames " + "will be published (need libEGL + a GPU/mesa device)"); + return; + } - // Head-pose segment for K1Sensors (the "NBPO" pose NUbridge publishes on the - // real robot). Torso tilt reaches NUbots only through this pose, so it is - // what makes fall detection / GetUp work in sim. - std::unique_ptr pose_writer; - if (!cfg.pose_segment.empty()) { + // Created from config alone (doesn't need the model), so the segment exists + // whether the sim or NUbots' K1Camera process starts first -- K1Camera + // retries opening it every 500 ms until it appears. + std::unique_ptr writer; try { - pose_writer = std::make_unique(cfg.pose_segment); + writer = std::make_unique(cfg.segment, cfg.width, cfg.height); } catch (const std::exception& e) { - log("Camera: failed to create head-pose segment", - cfg.pose_segment.c_str(), - ":", - e.what()); + log("Camera: failed to create shared-memory segment", + cfg.segment.c_str(), + ":", + e.what()); + return; } - } - const int head_body_id = mj_name2id(model, mjOBJ_BODY, "Head_2"); - if (pose_writer != nullptr && head_body_id < 0) { - log("Camera: body 'Head_2' not found — head pose will not be published"); - pose_writer.reset(); - } - - const int cam_id = mj_name2id(model, mjOBJ_CAMERA, cfg.mjcf_camera.c_str()); - if (cam_id < 0) { - log("Camera: mjcf camera", - cfg.mjcf_camera.c_str(), - "not found in the model -- check config/camera.yaml's mjcf_camera " - "against models/k1/K1_22dof.xml"); - return; - } - if (cfg.width > model->vis.global.offwidth || cfg.height > model->vis.global.offheight) { - log("Camera: configured", - cfg.width, - "x", - cfg.height, - "exceeds the model's compiled offscreen buffer", - model->vis.global.offwidth, - "x", - model->vis.global.offheight, - "-- fix config/camera.yaml or the model's " - ""); - return; - } + while (running_.load(std::memory_order_acquire) && !handles_ready_.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + if (!running_.load(std::memory_order_acquire)) { + return; + } - mjvCamera cam; - mjv_defaultCamera(&cam); - cam.type = mjCAMERA_FIXED; - cam.fixedcamid = cam_id; - - mjvOption opt; - mjv_defaultOption(&opt); - - mjvScene scn; - mjv_defaultScene(&scn); - mjv_makeScene(model, &scn, 2000); - - mjrContext con; - mjr_defaultContext(&con); - mjr_makeContext(model, &con, mjFONTSCALE_150); - mjr_setBuffer(mjFB_OFFSCREEN, &con); - - const mjrRect viewport{0, 0, cfg.width, cfg.height}; - std::vector raw(frame_bytes); // mjr_readPixels output: bottom-up (OpenGL convention) - std::vector flipped(frame_bytes); // top-down, matches what K1Camera/NUsight expect - - // Pinhole intrinsics derived from the MJCF camera's fovy + configured - // resolution (MuJoCo has no lens-distortion model, so k1/k2 published via - // SharedImageWriter::publish are always 0). `fov` follows the same - // "diagonal angle from the optical axis to the farthest image corner" - // convention module/platform/Webots.cpp used for its own simulated camera on - // the NUbots side (see utility::vision::projection's RECTILINEAR model and - // Webots.cpp's "auto fov" branch): unproject the far image corner and take - // twice that half-angle. - const double fovy_rad = model->cam_fovy[cam_id] * kPi / 180.0; - const double focal_px = (static_cast(cfg.height) * 0.5) / std::tan(fovy_rad * 0.5); - const double focal_length_norm = focal_px / static_cast(cfg.width); - const double aspect = static_cast(cfg.height) / static_cast(cfg.width); - const double half_diag_norm = 0.5 * std::sqrt(1.0 + aspect * aspect); - const double fov_rad = 2.0 * std::atan(half_diag_norm / focal_length_norm); - - log("Camera: rendering", - cfg.width, - "x", - cfg.height, - "fovy", - model->cam_fovy[cam_id], - "deg, focal_length_norm", - focal_length_norm, - "fov_diag_rad", - fov_rad); - - const auto frame_period = std::chrono::duration(1.0 / std::max(1.0, cfg.fps)); - auto next_frame = std::chrono::steady_clock::now(); - - while (running_.load(std::memory_order_acquire)) { - mjtNum head_p[3]{}; - mjtNum head_q[4]{1, 0, 0, 0}; // wxyz - mjtNum base_xy[2]{}; - mjtNum base_q[4]{1, 0, 0, 0}; // wxyz, free-joint qpos[3:7] - { - std::lock_guard lock(*sim_mutex); - mjv_updateScene(model, data, &opt, nullptr, &cam, mjCAT_ALL, &scn); - if (pose_writer != nullptr) { - for (int i = 0; i < 3; ++i) { - head_p[i] = data->xpos[3 * head_body_id + i]; - } - for (int i = 0; i < 4; ++i) { - head_q[i] = data->xquat[4 * head_body_id + i]; - base_q[i] = data->qpos[3 + i]; - } - base_xy[0] = data->qpos[0]; - base_xy[1] = data->qpos[1]; + const mjModel* model = handles_.model; + mjData* data = handles_.data; + std::mutex* sim_mutex = handles_.mutex; + + // Head-pose segment for K1Sensors (the "NBPO" pose NUbridge publishes on the + // real robot). Torso tilt reaches NUbots only through this pose, so it is + // what makes fall detection / GetUp work in sim. + std::unique_ptr pose_writer; + if (!cfg.pose_segment.empty()) { + try { + pose_writer = std::make_unique(cfg.pose_segment); + } + catch (const std::exception& e) { + log("Camera: failed to create head-pose segment", + cfg.pose_segment.c_str(), + ":", + e.what()); } } - if (pose_writer != nullptr) { - const FootprintPose Hrh = head_in_footprint(head_p, head_q, base_xy, base_q); - const double pos[3]{Hrh.position[0], Hrh.position[1], Hrh.position[2]}; - const double quat_xyzw[4]{Hrh.quat[1], Hrh.quat[2], Hrh.quat[3], Hrh.quat[0]}; - pose_writer->publish(pos, quat_xyzw); + const int head_body_id = mj_name2id(model, mjOBJ_BODY, "Head_2"); + if (pose_writer != nullptr && head_body_id < 0) { + log("Camera: body 'Head_2' not found — head pose will not be published"); + pose_writer.reset(); } - mjr_render(viewport, &scn, &con); - mjr_readPixels(raw.data(), nullptr, viewport, &con); - - // mjr_readPixels is bottom-up (OpenGL convention); K1Camera/NUsight - // expect top-down rgb8 like any normal image buffer, so flip row order - // once here rather than downstream. - for (int row = 0; row < cfg.height; ++row) { - const unsigned char* src = - raw.data() + static_cast(cfg.height - 1 - row) * static_cast(cfg.width) * 3; - unsigned char* dst = flipped.data() + static_cast(row) * static_cast(cfg.width) * 3; - std::memcpy(dst, src, static_cast(cfg.width) * 3); + + const int cam_id = mj_name2id(model, mjOBJ_CAMERA, cfg.mjcf_camera.c_str()); + if (cam_id < 0) { + log("Camera: mjcf camera", + cfg.mjcf_camera.c_str(), + "not found in the model -- check config/camera.yaml's mjcf_camera " + "against models/k1/K1_22dof.xml"); + return; } - writer->publish(flipped.data(), - flipped.size(), - static_cast(focal_length_norm), - static_cast(fov_rad), - 0.0f, - 0.0f); - - next_frame += std::chrono::duration_cast(frame_period); - const auto now = std::chrono::steady_clock::now(); - if (next_frame < now) { - next_frame = now; // fell behind -- resync instead of spinning to catch up + if (cfg.width > model->vis.global.offwidth || cfg.height > model->vis.global.offheight) { + log("Camera: configured", + cfg.width, + "x", + cfg.height, + "exceeds the model's compiled offscreen buffer", + model->vis.global.offwidth, + "x", + model->vis.global.offheight, + "-- fix config/camera.yaml or the model's " + ""); + return; } - std::this_thread::sleep_until(next_frame); - } - mjv_freeScene(&scn); - mjr_freeContext(&con); - writer.reset(); // destructor removes the shm segment - // EGL context torn down by egl's destructor (RAII), independent of Viewer's GLFW. -} + mjvCamera cam; + mjv_defaultCamera(&cam); + cam.type = mjCAMERA_FIXED; + cam.fixedcamid = cam_id; + + mjvOption opt; + mjv_defaultOption(&opt); + + mjvScene scn; + mjv_defaultScene(&scn); + mjv_makeScene(model, &scn, 2000); + + mjrContext con; + mjr_defaultContext(&con); + mjr_makeContext(model, &con, mjFONTSCALE_150); + mjr_setBuffer(mjFB_OFFSCREEN, &con); + + const mjrRect viewport{0, 0, cfg.width, cfg.height}; + std::vector raw(frame_bytes); // mjr_readPixels output: bottom-up (OpenGL convention) + std::vector flipped(frame_bytes); // top-down, matches what K1Camera/NUsight expect + + // Pinhole intrinsics derived from the MJCF camera's fovy + configured + // resolution (MuJoCo has no lens-distortion model, so k1/k2 published via + // SharedImageWriter::publish are always 0). `fov` follows the same + // "diagonal angle from the optical axis to the farthest image corner" + // convention module/platform/Webots.cpp used for its own simulated camera on + // the NUbots side (see utility::vision::projection's RECTILINEAR model and + // Webots.cpp's "auto fov" branch): unproject the far image corner and take + // twice that half-angle. + const double fovy_rad = model->cam_fovy[cam_id] * kPi / 180.0; + const double focal_px = (static_cast(cfg.height) * 0.5) / std::tan(fovy_rad * 0.5); + const double focal_length_norm = focal_px / static_cast(cfg.width); + const double aspect = static_cast(cfg.height) / static_cast(cfg.width); + const double half_diag_norm = 0.5 * std::sqrt(1.0 + aspect * aspect); + const double fov_rad = 2.0 * std::atan(half_diag_norm / focal_length_norm); + + log("Camera: rendering", + cfg.width, + "x", + cfg.height, + "fovy", + model->cam_fovy[cam_id], + "deg, focal_length_norm", + focal_length_norm, + "fov_diag_rad", + fov_rad); + + const auto frame_period = std::chrono::duration(1.0 / std::max(1.0, cfg.fps)); + auto next_frame = std::chrono::steady_clock::now(); + + while (running_.load(std::memory_order_acquire)) { + mjtNum head_p[3]{}; + mjtNum head_q[4]{1, 0, 0, 0}; // wxyz + mjtNum base_xy[2]{}; + mjtNum base_q[4]{1, 0, 0, 0}; // wxyz, free-joint qpos[3:7] + { + std::lock_guard lock(*sim_mutex); + mjv_updateScene(model, data, &opt, nullptr, &cam, mjCAT_ALL, &scn); + if (pose_writer != nullptr) { + for (int i = 0; i < 3; ++i) { + head_p[i] = data->xpos[3 * head_body_id + i]; + } + for (int i = 0; i < 4; ++i) { + head_q[i] = data->xquat[4 * head_body_id + i]; + base_q[i] = data->qpos[3 + i]; + } + base_xy[0] = data->qpos[0]; + base_xy[1] = data->qpos[1]; + } + } + if (pose_writer != nullptr) { + const FootprintPose Hrh = head_in_footprint(head_p, head_q, base_xy, base_q); + const double pos[3]{Hrh.position[0], Hrh.position[1], Hrh.position[2]}; + const double quat_xyzw[4]{Hrh.quat[1], Hrh.quat[2], Hrh.quat[3], Hrh.quat[0]}; + pose_writer->publish(pos, quat_xyzw); + } + mjr_render(viewport, &scn, &con); + mjr_readPixels(raw.data(), nullptr, viewport, &con); + + // mjr_readPixels is bottom-up (OpenGL convention); K1Camera/NUsight + // expect top-down rgb8 like any normal image buffer, so flip row order + // once here rather than downstream. + for (int row = 0; row < cfg.height; ++row) { + const unsigned char* src = + raw.data() + + static_cast(cfg.height - 1 - row) * static_cast(cfg.width) * 3; + unsigned char* dst = + flipped.data() + static_cast(row) * static_cast(cfg.width) * 3; + std::memcpy(dst, src, static_cast(cfg.width) * 3); + } + + writer->publish(flipped.data(), + flipped.size(), + static_cast(focal_length_norm), + static_cast(fov_rad), + 0.0f, + 0.0f); + + next_frame += std::chrono::duration_cast(frame_period); + const auto now = std::chrono::steady_clock::now(); + if (next_frame < now) { + next_frame = now; // fell behind -- resync instead of spinning to catch up + } + std::this_thread::sleep_until(next_frame); + } + + mjv_freeScene(&scn); + mjr_freeContext(&con); + writer.reset(); // destructor removes the shm segment + // EGL context torn down by egl's destructor (RAII), independent of Viewer's GLFW. + } } // namespace k1sim::module diff --git a/mujoco/module/Camera/src/Camera.hpp b/mujoco/module/Camera/src/Camera.hpp index c9fb852..4d96f46 100644 --- a/mujoco/module/Camera/src/Camera.hpp +++ b/mujoco/module/Camera/src/Camera.hpp @@ -10,41 +10,41 @@ namespace k1sim::module { -// Renders the K1's head camera offscreen (MuJoCo mjv/mjr) and publishes rgb8 -// frames into a Boost.Interprocess shared-memory segment matching NUbots' -// K1Camera SharedImageHeader byte-for-byte, so the unchanged input::K1Camera -// reads them -> ImageCompressor -> NetworkForwarder -> NUsight. No NUbots-side -// changes. -// -// Threading: all MuJoCo GL calls must happen on the one thread that holds the -// GL context current. module::Viewer already owns NUClear's MainThread for its -// own (window) GLFW context, and is disabled under --headless -- but Camera -// must keep rendering under --headless too (it feeds NUsight, not a local -// window), so it cannot piggyback on Viewer's MainThread reactions, and it must -// not gate on cli().headless the way Viewer does. Instead render_loop() runs on -// its own dedicated std::thread with its own hidden GLFW window/context, paced -// by its own sleep_until loop rather than on> (a NUClear thread-pool -// reaction is not guaranteed to run on the same OS thread twice, which would -// violate the one-thread-owns-the-context rule). See Camera.cpp for the full -// rationale, including why it deliberately never calls glfwTerminate(). -class Camera : public NUClear::Reactor { -public: - explicit Camera(std::unique_ptr environment); - -private: - void render_loop(camera::CameraConfig cfg); - - std::atomic running_{false}; - std::thread render_thread_; - - // Written once by the Trigger reaction (any NUClear pool - // thread); read by render_thread_ only after it observes handles_ready_ == - // true (release/acquire), which happens-before publishes the plain - // pointer/mutex writes in handles_ across threads -- module::Simulation - // only ever emits SimHandles once and never touches it again afterwards. - std::atomic handles_ready_{false}; - message::SimHandles handles_{}; -}; + // Renders the K1's head camera offscreen (MuJoCo mjv/mjr) and publishes rgb8 + // frames into a Boost.Interprocess shared-memory segment matching NUbots' + // K1Camera SharedImageHeader byte-for-byte, so the unchanged input::K1Camera + // reads them -> ImageCompressor -> NetworkForwarder -> NUsight. No NUbots-side + // changes. + // + // Threading: all MuJoCo GL calls must happen on the one thread that holds the + // GL context current. module::Viewer already owns NUClear's MainThread for its + // own (window) GLFW context, and is disabled under --headless -- but Camera + // must keep rendering under --headless too (it feeds NUsight, not a local + // window), so it cannot piggyback on Viewer's MainThread reactions, and it must + // not gate on cli().headless the way Viewer does. Instead render_loop() runs on + // its own dedicated std::thread with its own hidden GLFW window/context, paced + // by its own sleep_until loop rather than on> (a NUClear thread-pool + // reaction is not guaranteed to run on the same OS thread twice, which would + // violate the one-thread-owns-the-context rule). See Camera.cpp for the full + // rationale, including why it deliberately never calls glfwTerminate(). + class Camera : public NUClear::Reactor { + public: + explicit Camera(std::unique_ptr environment); + + private: + void render_loop(camera::CameraConfig cfg); + + std::atomic running_{false}; + std::thread render_thread_; + + // Written once by the Trigger reaction (any NUClear pool + // thread); read by render_thread_ only after it observes handles_ready_ == + // true (release/acquire), which happens-before publishes the plain + // pointer/mutex writes in handles_ across threads -- module::Simulation + // only ever emits SimHandles once and never touches it again afterwards. + std::atomic handles_ready_{false}; + message::SimHandles handles_{}; + }; } // namespace k1sim::module diff --git a/mujoco/module/Camera/src/CameraConfig.hpp b/mujoco/module/Camera/src/CameraConfig.hpp index 8c23dad..928b857 100644 --- a/mujoco/module/Camera/src/CameraConfig.hpp +++ b/mujoco/module/Camera/src/CameraConfig.hpp @@ -10,41 +10,41 @@ // hot-reload extension anywhere; config is read once at Startup). namespace k1sim::module::camera { -struct CameraConfig { - // POSIX shared-memory segment name. Must match the corresponding entry's - // `segment:` in NUbots_K1's module/input/K1Camera/data/config/K1Camera.yaml - // (default below matches that file's "Left Camera" entry). - std::string segment = "_boostercamera_head_rgb"; - - // Head-pose segment K1Sensors reads (K1Sensors.yaml `segment:`). Empty disables. - std::string pose_segment = "_head_pose"; - - // Render resolution. rgb8, so the segment carries width*height*3 pixel bytes - // after the header -- keep this under K1Camera's MAX_IMAGE_BYTES (2 MiB); the - // 640x480 default (921600 bytes) leaves plenty of margin. Must also not exceed - // the compiled model's (MuJoCo default - // 640x480, unmodified by any scene in this repo) -- Camera checks this at - // Startup and refuses to render rather than silently truncating. - int width = 640; - int height = 480; - - double fps = 30.0; - - // Name of the element the model attaches to the head (see - // models/k1/K1_22dof.xml's Head_2 body). - std::string mjcf_camera = "head"; -}; - -inline CameraConfig load_config(const YAML::Node& root) { - CameraConfig cfg; - cfg.segment = root["segment"].as(cfg.segment); - cfg.pose_segment = root["pose_segment"].as(cfg.pose_segment); - cfg.width = root["width"].as(cfg.width); - cfg.height = root["height"].as(cfg.height); - cfg.fps = root["fps"].as(cfg.fps); - cfg.mjcf_camera = root["mjcf_camera"].as(cfg.mjcf_camera); - return cfg; -} + struct CameraConfig { + // POSIX shared-memory segment name. Must match the corresponding entry's + // `segment:` in NUbots_K1's module/input/K1Camera/data/config/K1Camera.yaml + // (default below matches that file's "Left Camera" entry). + std::string segment = "_boostercamera_head_rgb"; + + // Head-pose segment K1Sensors reads (K1Sensors.yaml `segment:`). Empty disables. + std::string pose_segment = "_head_pose"; + + // Render resolution. rgb8, so the segment carries width*height*3 pixel bytes + // after the header -- keep this under K1Camera's MAX_IMAGE_BYTES (2 MiB); the + // 640x480 default (921600 bytes) leaves plenty of margin. Must also not exceed + // the compiled model's (MuJoCo default + // 640x480, unmodified by any scene in this repo) -- Camera checks this at + // Startup and refuses to render rather than silently truncating. + int width = 640; + int height = 480; + + double fps = 30.0; + + // Name of the element the model attaches to the head (see + // models/k1/K1_22dof.xml's Head_2 body). + std::string mjcf_camera = "head"; + }; + + inline CameraConfig load_config(const YAML::Node& root) { + CameraConfig cfg; + cfg.segment = root["segment"].as(cfg.segment); + cfg.pose_segment = root["pose_segment"].as(cfg.pose_segment); + cfg.width = root["width"].as(cfg.width); + cfg.height = root["height"].as(cfg.height); + cfg.fps = root["fps"].as(cfg.fps); + cfg.mjcf_camera = root["mjcf_camera"].as(cfg.mjcf_camera); + return cfg; + } } // namespace k1sim::module::camera diff --git a/mujoco/module/Camera/src/EglContext.hpp b/mujoco/module/Camera/src/EglContext.hpp index e35561d..6e4c7e7 100644 --- a/mujoco/module/Camera/src/EglContext.hpp +++ b/mujoco/module/Camera/src/EglContext.hpp @@ -5,83 +5,89 @@ namespace k1sim::module::camera { -// RAII offscreen OpenGL context via EGL (pbuffer). Camera uses this instead of a -// hidden GLFW window so it shares NO global GLFW/Xlib state with module::Viewer's -// on-screen GLFW window — the two ran on different threads and concurrent GLFW/ -// Xlib calls corrupted the heap ("malloc(): invalid size"). EGL also works with -// no display at all, so the camera can render headless. MuJoCo's mjr renders into -// whatever GL context is current, so making this current before mjr_makeContext is -// all that's needed (the MUJOCO_GL=egl pattern from MuJoCo's own examples). -class EglContext { -public: - EglContext(int width, int height) { - display_ = eglGetDisplay(EGL_DEFAULT_DISPLAY); - if (display_ == EGL_NO_DISPLAY) { - return; + // RAII offscreen OpenGL context via EGL (pbuffer). Camera uses this instead of a + // hidden GLFW window so it shares NO global GLFW/Xlib state with module::Viewer's + // on-screen GLFW window — the two ran on different threads and concurrent GLFW/ + // Xlib calls corrupted the heap ("malloc(): invalid size"). EGL also works with + // no display at all, so the camera can render headless. MuJoCo's mjr renders into + // whatever GL context is current, so making this current before mjr_makeContext is + // all that's needed (the MUJOCO_GL=egl pattern from MuJoCo's own examples). + class EglContext { + public: + EglContext(int width, int height) { + display_ = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (display_ == EGL_NO_DISPLAY) { + return; + } + EGLint major = 0; + EGLint minor = 0; + if (eglInitialize(display_, &major, &minor) == EGL_FALSE) { + display_ = EGL_NO_DISPLAY; + return; + } + const EGLint config_attr[] = {EGL_SURFACE_TYPE, + EGL_PBUFFER_BIT, + EGL_RED_SIZE, + 8, + EGL_GREEN_SIZE, + 8, + EGL_BLUE_SIZE, + 8, + EGL_DEPTH_SIZE, + 24, + EGL_RENDERABLE_TYPE, + EGL_OPENGL_BIT, + EGL_NONE}; + EGLConfig config = nullptr; + EGLint num_config = 0; + if (eglChooseConfig(display_, config_attr, &config, 1, &num_config) == EGL_FALSE || num_config < 1) { + return; + } + const EGLint pbuffer_attr[] = {EGL_WIDTH, width, EGL_HEIGHT, height, EGL_NONE}; + surface_ = eglCreatePbufferSurface(display_, config, pbuffer_attr); + if (surface_ == EGL_NO_SURFACE) { + return; + } + if (eglBindAPI(EGL_OPENGL_API) == EGL_FALSE) { + return; + } + context_ = eglCreateContext(display_, config, EGL_NO_CONTEXT, nullptr); + if (context_ == EGL_NO_CONTEXT) { + return; + } + if (eglMakeCurrent(display_, surface_, surface_, context_) == EGL_FALSE) { + return; + } + valid_ = true; } - EGLint major = 0; - EGLint minor = 0; - if (eglInitialize(display_, &major, &minor) == EGL_FALSE) { - display_ = EGL_NO_DISPLAY; - return; - } - const EGLint config_attr[] = {EGL_SURFACE_TYPE, EGL_PBUFFER_BIT, - EGL_RED_SIZE, 8, - EGL_GREEN_SIZE, 8, - EGL_BLUE_SIZE, 8, - EGL_DEPTH_SIZE, 24, - EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT, - EGL_NONE}; - EGLConfig config = nullptr; - EGLint num_config = 0; - if (eglChooseConfig(display_, config_attr, &config, 1, &num_config) == EGL_FALSE || num_config < 1) { - return; - } - const EGLint pbuffer_attr[] = {EGL_WIDTH, width, EGL_HEIGHT, height, EGL_NONE}; - surface_ = eglCreatePbufferSurface(display_, config, pbuffer_attr); - if (surface_ == EGL_NO_SURFACE) { - return; - } - if (eglBindAPI(EGL_OPENGL_API) == EGL_FALSE) { - return; - } - context_ = eglCreateContext(display_, config, EGL_NO_CONTEXT, nullptr); - if (context_ == EGL_NO_CONTEXT) { - return; - } - if (eglMakeCurrent(display_, surface_, surface_, context_) == EGL_FALSE) { - return; - } - valid_ = true; - } - ~EglContext() { - if (display_ == EGL_NO_DISPLAY) { - return; - } - eglMakeCurrent(display_, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); - if (context_ != EGL_NO_CONTEXT) { - eglDestroyContext(display_, context_); + ~EglContext() { + if (display_ == EGL_NO_DISPLAY) { + return; + } + eglMakeCurrent(display_, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); + if (context_ != EGL_NO_CONTEXT) { + eglDestroyContext(display_, context_); + } + if (surface_ != EGL_NO_SURFACE) { + eglDestroySurface(display_, surface_); + } + eglTerminate(display_); } - if (surface_ != EGL_NO_SURFACE) { - eglDestroySurface(display_, surface_); - } - eglTerminate(display_); - } - bool valid() const { - return valid_; - } + bool valid() const { + return valid_; + } - EglContext(const EglContext&) = delete; - EglContext& operator=(const EglContext&) = delete; + EglContext(const EglContext&) = delete; + EglContext& operator=(const EglContext&) = delete; -private: - EGLDisplay display_ = EGL_NO_DISPLAY; - EGLContext context_ = EGL_NO_CONTEXT; - EGLSurface surface_ = EGL_NO_SURFACE; - bool valid_ = false; -}; + private: + EGLDisplay display_ = EGL_NO_DISPLAY; + EGLContext context_ = EGL_NO_CONTEXT; + EGLSurface surface_ = EGL_NO_SURFACE; + bool valid_ = false; + }; } // namespace k1sim::module::camera diff --git a/mujoco/module/Camera/src/SharedImageWriter.cpp b/mujoco/module/Camera/src/SharedImageWriter.cpp index d3de8ea..dea64e2 100644 --- a/mujoco/module/Camera/src/SharedImageWriter.cpp +++ b/mujoco/module/Camera/src/SharedImageWriter.cpp @@ -7,78 +7,78 @@ namespace k1sim::module::camera { -namespace { -constexpr const char* kEncoding = "rgb8"; -} - -SharedImageWriter::SharedImageWriter(const std::string& segment_name, int width, int height) - : segment_name_(segment_name) { - - if (width <= 0 || height <= 0) { - throw std::runtime_error("SharedImageWriter: width/height must be positive"); + namespace { + constexpr const char* kEncoding = "rgb8"; } - const auto data_size = static_cast(width) * static_cast(height) * 3; - const auto total_size = sizeof(SharedImageHeader) + data_size; + SharedImageWriter::SharedImageWriter(const std::string& segment_name, int width, int height) + : segment_name_(segment_name) { + + if (width <= 0 || height <= 0) { + throw std::runtime_error("SharedImageWriter: width/height must be positive"); + } - // Discard any stale segment (e.g. left behind by a crashed previous run) so we - // never in-place-construct a mutex/condition over memory a stuck waiter might - // still reference, and so size always matches width/height. remove() is a - // no-op (returns false, doesn't throw) if nothing was there. - bip::shared_memory_object::remove(segment_name_.c_str()); + const auto data_size = static_cast(width) * static_cast(height) * 3; + const auto total_size = sizeof(SharedImageHeader) + data_size; - shm_ = bip::shared_memory_object(bip::create_only, segment_name_.c_str(), bip::read_write); - shm_.truncate(static_cast(total_size)); - region_ = bip::mapped_region(shm_, bip::read_write); + // Discard any stale segment (e.g. left behind by a crashed previous run) so we + // never in-place-construct a mutex/condition over memory a stuck waiter might + // still reference, and so size always matches width/height. remove() is a + // no-op (returns false, doesn't throw) if nothing was there. + bip::shared_memory_object::remove(segment_name_.c_str()); - // Placement-new the header on the freshly-mapped memory so its members (incl. - // the seqlock atomic) are initialised exactly once (seqlock = 0 = "no frame yet"). - header_ = new (region_.get_address()) SharedImageHeader(); - header_->width = static_cast(width); - header_->height = static_cast(height); - header_->data_size = static_cast(data_size); - // encoding[] is already zero-filled by SharedImageHeader's default member - // initializer; just copy the (shorter, NUL-terminated-by-the-zero-fill) tag in. - std::memcpy(header_->encoding, kEncoding, std::strlen(kEncoding)); + shm_ = bip::shared_memory_object(bip::create_only, segment_name_.c_str(), bip::read_write); + shm_.truncate(static_cast(total_size)); + region_ = bip::mapped_region(shm_, bip::read_write); - pixels_ = reinterpret_cast(header_ + 1); -} + // Placement-new the header on the freshly-mapped memory so its members (incl. + // the seqlock atomic) are initialised exactly once (seqlock = 0 = "no frame yet"). + header_ = new (region_.get_address()) SharedImageHeader(); + header_->width = static_cast(width); + header_->height = static_cast(height); + header_->data_size = static_cast(data_size); + // encoding[] is already zero-filled by SharedImageHeader's default member + // initializer; just copy the (shorter, NUL-terminated-by-the-zero-fill) tag in. + std::memcpy(header_->encoding, kEncoding, std::strlen(kEncoding)); -SharedImageWriter::~SharedImageWriter() { - if (header_ != nullptr) { - header_->~SharedImageHeader(); + pixels_ = reinterpret_cast(header_ + 1); } - bip::shared_memory_object::remove(segment_name_.c_str()); -} -void SharedImageWriter::publish(const uint8_t* rgb, - std::size_t rgb_size, - float focal_length, - float fov, - float centre_x, - float centre_y) { - if (header_ == nullptr || rgb_size != header_->data_size) { - return; + SharedImageWriter::~SharedImageWriter() { + if (header_ != nullptr) { + header_->~SharedImageHeader(); + } + bip::shared_memory_object::remove(segment_name_.c_str()); } - // Seqlock publish (single writer). Bump odd = "write in progress", write the - // frame + intrinsics, bump even = "frame N published" (N = seqlock/2). A reader - // that snapshots seqlock before and after its copy sees an odd or changed value - // if it raced this write, and retries. No lock is ever held, so a reader that - // dies mid-read cannot wedge this writer. - const uint64_t s = header_->seqlock.load(std::memory_order_relaxed); - header_->seqlock.store(s + 1, std::memory_order_release); // odd: write in progress - std::atomic_thread_fence(std::memory_order_release); - std::memcpy(pixels_, rgb, rgb_size); - header_->focal_length = focal_length; - header_->fov = fov; - header_->centre_x = centre_x; - header_->centre_y = centre_y; - header_->k1 = 0.0f; - header_->k2 = 0.0f; + void SharedImageWriter::publish(const uint8_t* rgb, + std::size_t rgb_size, + float focal_length, + float fov, + float centre_x, + float centre_y) { + if (header_ == nullptr || rgb_size != header_->data_size) { + return; + } + // Seqlock publish (single writer). Bump odd = "write in progress", write the + // frame + intrinsics, bump even = "frame N published" (N = seqlock/2). A reader + // that snapshots seqlock before and after its copy sees an odd or changed value + // if it raced this write, and retries. No lock is ever held, so a reader that + // dies mid-read cannot wedge this writer. + const uint64_t s = header_->seqlock.load(std::memory_order_relaxed); + header_->seqlock.store(s + 1, std::memory_order_release); // odd: write in progress + std::atomic_thread_fence(std::memory_order_release); - std::atomic_thread_fence(std::memory_order_release); - header_->seqlock.store(s + 2, std::memory_order_release); // even: frame published -} + std::memcpy(pixels_, rgb, rgb_size); + header_->focal_length = focal_length; + header_->fov = fov; + header_->centre_x = centre_x; + header_->centre_y = centre_y; + header_->k1 = 0.0f; + header_->k2 = 0.0f; + + std::atomic_thread_fence(std::memory_order_release); + header_->seqlock.store(s + 2, std::memory_order_release); // even: frame published + } } // namespace k1sim::module::camera diff --git a/mujoco/module/Camera/src/SharedImageWriter.hpp b/mujoco/module/Camera/src/SharedImageWriter.hpp index 8b8862a..549d932 100644 --- a/mujoco/module/Camera/src/SharedImageWriter.hpp +++ b/mujoco/module/Camera/src/SharedImageWriter.hpp @@ -10,86 +10,86 @@ namespace k1sim::module::camera { -namespace bip = boost::interprocess; + namespace bip = boost::interprocess; -// Byte-for-byte identical to the reader-side copy in NUbots_K1's -// module/input/K1Camera/src/K1Camera.cpp. That struct lives in a different -// repo/build -- there is no shared header to #include, so this copy *is* the -// contract. If you change one, change the other; field order/types must match -// exactly (header first, so `this + 1` is the first pixel byte). -// -// Lock-free seqlock (VERSION 2): the old layout used a boost interprocess_mutex -// + condition that both writer and reader locked. That mutex is not robust, so a -// consumer (K1Camera) killed while holding it left the lock stuck in the segment, -// blocking the next reader AND this writer -- the "one frame then stall on -// behaviour restart" bug. `seqlock` replaces them: the writer bumps it odd before -// writing a frame and even after (frame number = seqlock/2); a reader snapshots it -// before and after copying and retries if it changed or was odd. Nothing is ever -// locked, so killing either side leaves the segment usable -- behaviour can restart -// without restarting the sim. -struct SharedImageHeader { - // magic/version lead the struct so a mismatched build is detected instead of - // misread. They sit at the same offset as the old layout, so a v2 reader can - // still read them off a stale v1 segment and reject it. - static constexpr uint32_t MAGIC = 0x4E42494D; // "NBIM" - static constexpr uint32_t VERSION = 2; - uint32_t magic{MAGIC}; - uint32_t version{VERSION}; - // Seqlock: even = a complete frame is published; odd = a write is in progress. - std::atomic seqlock{0}; - uint32_t data_size{0}; - uint32_t width{0}; - uint32_t height{0}; - char encoding[32]{}; - float focal_length{0.0f}; - float fov{0.0f}; - float centre_x{0.0f}; - float centre_y{0.0f}; - float k1{0.0f}; - float k2{0.0f}; -}; + // Byte-for-byte identical to the reader-side copy in NUbots_K1's + // module/input/K1Camera/src/K1Camera.cpp. That struct lives in a different + // repo/build -- there is no shared header to #include, so this copy *is* the + // contract. If you change one, change the other; field order/types must match + // exactly (header first, so `this + 1` is the first pixel byte). + // + // Lock-free seqlock (VERSION 2): the old layout used a boost interprocess_mutex + // + condition that both writer and reader locked. That mutex is not robust, so a + // consumer (K1Camera) killed while holding it left the lock stuck in the segment, + // blocking the next reader AND this writer -- the "one frame then stall on + // behaviour restart" bug. `seqlock` replaces them: the writer bumps it odd before + // writing a frame and even after (frame number = seqlock/2); a reader snapshots it + // before and after copying and retries if it changed or was odd. Nothing is ever + // locked, so killing either side leaves the segment usable -- behaviour can restart + // without restarting the sim. + struct SharedImageHeader { + // magic/version lead the struct so a mismatched build is detected instead of + // misread. They sit at the same offset as the old layout, so a v2 reader can + // still read them off a stale v1 segment and reject it. + static constexpr uint32_t MAGIC = 0x4E42494D; // "NBIM" + static constexpr uint32_t VERSION = 2; + uint32_t magic{MAGIC}; + uint32_t version{VERSION}; + // Seqlock: even = a complete frame is published; odd = a write is in progress. + std::atomic seqlock{0}; + uint32_t data_size{0}; + uint32_t width{0}; + uint32_t height{0}; + char encoding[32]{}; + float focal_length{0.0f}; + float fov{0.0f}; + float centre_x{0.0f}; + float centre_y{0.0f}; + float k1{0.0f}; + float k2{0.0f}; + }; -// Owns the POSIX shared-memory segment NUbots' input::K1Camera reads. On -// construction, (re)creates the segment sized for width x height rgb8 -- any -// stale segment from a previous run is removed first, so the seqlock always -// starts at 0 and the size always matches the current config. The segment (and -// the in-place-constructed header) exists as soon as this constructor returns, -// independent of whether the MuJoCo model has loaded yet -- K1Camera retries -// opening it every 500 ms until it appears, so creation order with NUbots -// doesn't matter. -// -// Not thread-safe against concurrent publish() calls; Camera only ever calls -// it from its one dedicated render thread. -class SharedImageWriter { -public: - // Throws std::runtime_error (width/height <= 0) or - // boost::interprocess::interprocess_exception (shm create/map failure). - SharedImageWriter(const std::string& segment_name, int width, int height); - ~SharedImageWriter(); + // Owns the POSIX shared-memory segment NUbots' input::K1Camera reads. On + // construction, (re)creates the segment sized for width x height rgb8 -- any + // stale segment from a previous run is removed first, so the seqlock always + // starts at 0 and the size always matches the current config. The segment (and + // the in-place-constructed header) exists as soon as this constructor returns, + // independent of whether the MuJoCo model has loaded yet -- K1Camera retries + // opening it every 500 ms until it appears, so creation order with NUbots + // doesn't matter. + // + // Not thread-safe against concurrent publish() calls; Camera only ever calls + // it from its one dedicated render thread. + class SharedImageWriter { + public: + // Throws std::runtime_error (width/height <= 0) or + // boost::interprocess::interprocess_exception (shm create/map failure). + SharedImageWriter(const std::string& segment_name, int width, int height); + ~SharedImageWriter(); - SharedImageWriter(const SharedImageWriter&) = delete; - SharedImageWriter& operator=(const SharedImageWriter&) = delete; + SharedImageWriter(const SharedImageWriter&) = delete; + SharedImageWriter& operator=(const SharedImageWriter&) = delete; - // `rgb` must be exactly width*height*3 bytes (rgb8), top-down row order (row 0 - // = top of image) -- matches ros_encoding_to_fourcc("rgb8") on the reader - // side. Bumps `seqlock` odd (write in progress), copies the pixels + intrinsics - // fields, then bumps it even (frame published) -- no locks. No-op (defensive - // only -- rgb_size is always frame_bytes by construction on the one call site) - // if rgb_size doesn't match the segment's data_size. - void publish(const uint8_t* rgb, - std::size_t rgb_size, - float focal_length, - float fov, - float centre_x, - float centre_y); + // `rgb` must be exactly width*height*3 bytes (rgb8), top-down row order (row 0 + // = top of image) -- matches ros_encoding_to_fourcc("rgb8") on the reader + // side. Bumps `seqlock` odd (write in progress), copies the pixels + intrinsics + // fields, then bumps it even (frame published) -- no locks. No-op (defensive + // only -- rgb_size is always frame_bytes by construction on the one call site) + // if rgb_size doesn't match the segment's data_size. + void publish(const uint8_t* rgb, + std::size_t rgb_size, + float focal_length, + float fov, + float centre_x, + float centre_y); -private: - std::string segment_name_; - bip::shared_memory_object shm_; - bip::mapped_region region_; - SharedImageHeader* header_ = nullptr; // = region_.get_address(), once mapped - uint8_t* pixels_ = nullptr; // = header_ + 1 -}; + private: + std::string segment_name_; + bip::shared_memory_object shm_; + bip::mapped_region region_; + SharedImageHeader* header_ = nullptr; // = region_.get_address(), once mapped + uint8_t* pixels_ = nullptr; // = header_ + 1 + }; } // namespace k1sim::module::camera diff --git a/mujoco/module/Camera/src/SharedPoseWriter.cpp b/mujoco/module/Camera/src/SharedPoseWriter.cpp index 5f8aaa6..59f5e6e 100644 --- a/mujoco/module/Camera/src/SharedPoseWriter.cpp +++ b/mujoco/module/Camera/src/SharedPoseWriter.cpp @@ -5,40 +5,40 @@ namespace k1sim::module::camera { -SharedPoseWriter::SharedPoseWriter(const std::string& segment_name) : segment_name_(segment_name) { - // Same rationale as SharedImageWriter: never in-place-construct over a - // stale segment a crashed run's reader might still reference. - bip::shared_memory_object::remove(segment_name_.c_str()); + SharedPoseWriter::SharedPoseWriter(const std::string& segment_name) : segment_name_(segment_name) { + // Same rationale as SharedImageWriter: never in-place-construct over a + // stale segment a crashed run's reader might still reference. + bip::shared_memory_object::remove(segment_name_.c_str()); - shm_ = bip::shared_memory_object(bip::create_only, segment_name_.c_str(), bip::read_write); - shm_.truncate(static_cast(sizeof(SharedPoseHeader))); - region_ = bip::mapped_region(shm_, bip::read_write); + shm_ = bip::shared_memory_object(bip::create_only, segment_name_.c_str(), bip::read_write); + shm_.truncate(static_cast(sizeof(SharedPoseHeader))); + region_ = bip::mapped_region(shm_, bip::read_write); - header_ = new (region_.get_address()) SharedPoseHeader(); -} - -SharedPoseWriter::~SharedPoseWriter() { - if (header_ != nullptr) { - header_->~SharedPoseHeader(); + header_ = new (region_.get_address()) SharedPoseHeader(); } - bip::shared_memory_object::remove(segment_name_.c_str()); -} -void SharedPoseWriter::publish(const double position[3], const double orientation_xyzw[4]) { - if (header_ == nullptr) { - return; + SharedPoseWriter::~SharedPoseWriter() { + if (header_ != nullptr) { + header_->~SharedPoseHeader(); + } + bip::shared_memory_object::remove(segment_name_.c_str()); } - { - bip::scoped_lock lock(header_->mutex); - for (int i = 0; i < 3; ++i) { - header_->position[i] = position[i]; + + void SharedPoseWriter::publish(const double position[3], const double orientation_xyzw[4]) { + if (header_ == nullptr) { + return; } - for (int i = 0; i < 4; ++i) { - header_->orientation[i] = orientation_xyzw[i]; + { + bip::scoped_lock lock(header_->mutex); + for (int i = 0; i < 3; ++i) { + header_->position[i] = position[i]; + } + for (int i = 0; i < 4; ++i) { + header_->orientation[i] = orientation_xyzw[i]; + } + ++header_->sequence; } - ++header_->sequence; + header_->has_new_data.notify_all(); } - header_->has_new_data.notify_all(); -} } // namespace k1sim::module::camera diff --git a/mujoco/module/Camera/src/SharedPoseWriter.hpp b/mujoco/module/Camera/src/SharedPoseWriter.hpp index 870d4c5..a2b0c88 100644 --- a/mujoco/module/Camera/src/SharedPoseWriter.hpp +++ b/mujoco/module/Camera/src/SharedPoseWriter.hpp @@ -10,48 +10,48 @@ namespace k1sim::module::camera { -namespace bip = boost::interprocess; - -// Byte-for-byte match of NUbots' input::K1Sensors SharedPoseHeader ("NBPO", -// see module/input/K1Sensors/src/K1Sensors.cpp) — the head-pose segment -// NUbridge publishes on the real robot. K1Sensors folds this pose into -// Sensors.Htw, which is the only place torso tilt enters the NUbots side -// (odometry is yaw-only), so without this segment fall detection — and -// therefore the whole GetUp chain — never fires. -struct SharedPoseHeader { - static constexpr uint32_t MAGIC = 0x4E42504F; // "NBPO" - static constexpr uint32_t VERSION = 1; - uint32_t magic{MAGIC}; - uint32_t version{VERSION}; - bip::interprocess_mutex mutex; - bip::interprocess_condition has_new_data; - uint64_t sequence{0}; - double position[3]{0.0, 0.0, 0.0}; - double orientation[4]{0.0, 0.0, 0.0, 1.0}; // xyzw -}; - -// Owns the POSIX shared-memory segment K1Sensors reads. Same lifecycle rules -// as SharedImageWriter: stale segments removed on construction, segment -// removed again on destruction, single-writer only. -class SharedPoseWriter { -public: - explicit SharedPoseWriter(const std::string& segment_name); - ~SharedPoseWriter(); - - SharedPoseWriter(const SharedPoseWriter&) = delete; - SharedPoseWriter& operator=(const SharedPoseWriter&) = delete; - - // position: metres; orientation: quaternion xyzw. Frame contract matches - // NUbridge: head (pitch-link) pose in the yaw-only base footprint frame, - // i.e. Hrh = (translate(base_x, base_y, 0) * rotz(base_yaw))^-1 * Hwh. - void publish(const double position[3], const double orientation_xyzw[4]); - -private: - std::string segment_name_; - bip::shared_memory_object shm_; - bip::mapped_region region_; - SharedPoseHeader* header_ = nullptr; -}; + namespace bip = boost::interprocess; + + // Byte-for-byte match of NUbots' input::K1Sensors SharedPoseHeader ("NBPO", + // see module/input/K1Sensors/src/K1Sensors.cpp) — the head-pose segment + // NUbridge publishes on the real robot. K1Sensors folds this pose into + // Sensors.Htw, which is the only place torso tilt enters the NUbots side + // (odometry is yaw-only), so without this segment fall detection — and + // therefore the whole GetUp chain — never fires. + struct SharedPoseHeader { + static constexpr uint32_t MAGIC = 0x4E42504F; // "NBPO" + static constexpr uint32_t VERSION = 1; + uint32_t magic{MAGIC}; + uint32_t version{VERSION}; + bip::interprocess_mutex mutex; + bip::interprocess_condition has_new_data; + uint64_t sequence{0}; + double position[3]{0.0, 0.0, 0.0}; + double orientation[4]{0.0, 0.0, 0.0, 1.0}; // xyzw + }; + + // Owns the POSIX shared-memory segment K1Sensors reads. Same lifecycle rules + // as SharedImageWriter: stale segments removed on construction, segment + // removed again on destruction, single-writer only. + class SharedPoseWriter { + public: + explicit SharedPoseWriter(const std::string& segment_name); + ~SharedPoseWriter(); + + SharedPoseWriter(const SharedPoseWriter&) = delete; + SharedPoseWriter& operator=(const SharedPoseWriter&) = delete; + + // position: metres; orientation: quaternion xyzw. Frame contract matches + // NUbridge: head (pitch-link) pose in the yaw-only base footprint frame, + // i.e. Hrh = (translate(base_x, base_y, 0) * rotz(base_yaw))^-1 * Hwh. + void publish(const double position[3], const double orientation_xyzw[4]); + + private: + std::string segment_name_; + bip::shared_memory_object shm_; + bip::mapped_region region_; + SharedPoseHeader* header_ = nullptr; + }; } // namespace k1sim::module::camera diff --git a/mujoco/module/ConsoleLog/src/ConsoleLog.cpp b/mujoco/module/ConsoleLog/src/ConsoleLog.cpp index 74d0293..dc574b3 100644 --- a/mujoco/module/ConsoleLog/src/ConsoleLog.cpp +++ b/mujoco/module/ConsoleLog/src/ConsoleLog.cpp @@ -5,16 +5,16 @@ namespace k1sim::module { -ConsoleLog::ConsoleLog(std::unique_ptr environment) : Reactor(std::move(environment)) { + ConsoleLog::ConsoleLog(std::unique_ptr environment) : Reactor(std::move(environment)) { - on>().then([](const NUClear::message::LogMessage& msg) { - if (msg.level < msg.display_level) { - return; - } - const std::string level = msg.level; // LogLevel has a string conversion operator - std::printf("[%s] %s: %s\n", level.c_str(), msg.reactor_name.c_str(), msg.message.c_str()); - std::fflush(stdout); - }); -} + on>().then([](const NUClear::message::LogMessage& msg) { + if (msg.level < msg.display_level) { + return; + } + const std::string level = msg.level; // LogLevel has a string conversion operator + std::printf("[%s] %s: %s\n", level.c_str(), msg.reactor_name.c_str(), msg.message.c_str()); + std::fflush(stdout); + }); + } } // namespace k1sim::module diff --git a/mujoco/module/ConsoleLog/src/ConsoleLog.hpp b/mujoco/module/ConsoleLog/src/ConsoleLog.hpp index 55023f0..02c9a3f 100644 --- a/mujoco/module/ConsoleLog/src/ConsoleLog.hpp +++ b/mujoco/module/ConsoleLog/src/ConsoleLog.hpp @@ -5,12 +5,12 @@ namespace k1sim::module { -// Minimal console sink for NUClear log() messages (NUClear emits LogMessage; -// without a handler reactor, logs go nowhere). -class ConsoleLog : public NUClear::Reactor { -public: - explicit ConsoleLog(std::unique_ptr environment); -}; + // Minimal console sink for NUClear log() messages (NUClear emits LogMessage; + // without a handler reactor, logs go nowhere). + class ConsoleLog : public NUClear::Reactor { + public: + explicit ConsoleLog(std::unique_ptr environment); + }; } // namespace k1sim::module diff --git a/mujoco/module/Locomotion/CMakeLists.txt b/mujoco/module/Locomotion/CMakeLists.txt index da314fd..ce86b27 100644 --- a/mujoco/module/Locomotion/CMakeLists.txt +++ b/mujoco/module/Locomotion/CMakeLists.txt @@ -1,6 +1,2 @@ -add_library( - k1sim_module_locomotion STATIC - src/Locomotion.cpp - src/LocomotionController.cpp -) +add_library(k1sim_module_locomotion STATIC src/Locomotion.cpp src/LocomotionController.cpp) target_link_libraries(k1sim_module_locomotion PUBLIC k1sim_shared) diff --git a/mujoco/module/Locomotion/src/LocoMath.hpp b/mujoco/module/Locomotion/src/LocoMath.hpp index c4764a7..ebfe672 100644 --- a/mujoco/module/Locomotion/src/LocoMath.hpp +++ b/mujoco/module/Locomotion/src/LocoMath.hpp @@ -15,31 +15,31 @@ namespace k1sim::module { -// Smooth 0->1 ease (zero derivative at both ends) -- the Prepare blend shape. -inline double smoothstep(double t) { - t = std::clamp(t, 0.0, 1.0); - return t * t * (3.0 - 2.0 * t); -} - -inline double lerp(double a, double b, double t) { - return a + (b - a) * t; -} - -// The body's +z axis expressed in world coordinates ("up" if standing). -inline std::array base_up_vector(const mjData* d, const ModelMap& map) { - const double* quat = &d->qpos[map.root_qpos_adr + 3]; // wxyz - std::array z_axis{0.0, 0.0, 1.0}; - std::array up{}; - mju_rotVecQuat(up.data(), z_axis.data(), quat); - return up; -} - -// Angle (rad) between the base's up vector and world-up. 0 = perfectly -// upright, pi/2 = lying on its side, pi = upside down. -inline double base_tilt(const mjData* d, const ModelMap& map) { - const auto up = base_up_vector(d, map); - return std::acos(std::clamp(up[2], -1.0, 1.0)); -} + // Smooth 0->1 ease (zero derivative at both ends) -- the Prepare blend shape. + inline double smoothstep(double t) { + t = std::clamp(t, 0.0, 1.0); + return t * t * (3.0 - 2.0 * t); + } + + inline double lerp(double a, double b, double t) { + return a + (b - a) * t; + } + + // The body's +z axis expressed in world coordinates ("up" if standing). + inline std::array base_up_vector(const mjData* d, const ModelMap& map) { + const double* quat = &d->qpos[map.root_qpos_adr + 3]; // wxyz + std::array z_axis{0.0, 0.0, 1.0}; + std::array up{}; + mju_rotVecQuat(up.data(), z_axis.data(), quat); + return up; + } + + // Angle (rad) between the base's up vector and world-up. 0 = perfectly + // upright, pi/2 = lying on its side, pi = upside down. + inline double base_tilt(const mjData* d, const ModelMap& map) { + const auto up = base_up_vector(d, map); + return std::acos(std::clamp(up[2], -1.0, 1.0)); + } } // namespace k1sim::module diff --git a/mujoco/module/Locomotion/src/Locomotion.cpp b/mujoco/module/Locomotion/src/Locomotion.cpp index 374d5e6..af2f3aa 100644 --- a/mujoco/module/Locomotion/src/Locomotion.cpp +++ b/mujoco/module/Locomotion/src/Locomotion.cpp @@ -6,72 +6,63 @@ namespace k1sim::module { -using message::ControllerHandle; -using message::GetUpRequest; -using message::HeadCommand; -using message::LieDownRequest; -using message::LowCmdMessage; -using message::ModeChangeRequest; -using message::VisualKickRequest; -using message::WalkCommand; + using message::ControllerHandle; + using message::GetUpRequest; + using message::HeadCommand; + using message::LieDownRequest; + using message::LowCmdMessage; + using message::ModeChangeRequest; + using message::VisualKickRequest; + using message::WalkCommand; -Locomotion::Locomotion(std::unique_ptr environment) : Reactor(std::move(environment)) { + Locomotion::Locomotion(std::unique_ptr environment) : Reactor(std::move(environment)) { - on().then([this] { - auto locomotion_cfg = config::load("locomotion.yaml"); - auto gains_cfg = config::load("gains.yaml"); + on().then([this] { + auto locomotion_cfg = config::load("locomotion.yaml"); + auto gains_cfg = config::load("gains.yaml"); - try { - controller_ = std::make_unique(locomotion_cfg, gains_cfg); - } - catch (const std::exception& e) { - log("Locomotion: failed to build the mode controller:", e.what()); - throw; - } + try { + controller_ = std::make_unique(locomotion_cfg, gains_cfg); + } + catch (const std::exception& e) { + log("Locomotion: failed to build the mode controller:", e.what()); + throw; + } - emit(std::make_unique(ControllerHandle{controller_.get()})); - log("Locomotion ready (servo-command listener; policies live in NUbots_K1)"); - }); + emit(std::make_unique(ControllerHandle{controller_.get()})); + log("Locomotion ready (servo-command listener; policies live in NUbots_K1)"); + }); - on>().then([this](const HeadCommand& cmd) { - controller_->set_head_command(cmd.pitch, cmd.yaw); - }); + on>().then( + [this](const HeadCommand& cmd) { controller_->set_head_command(cmd.pitch, cmd.yaw); }); - on>().then([this](const ModeChangeRequest& req) { - log("ChangeMode requested: mode", req.mode); - controller_->request_mode_change(req.mode); - }); + on>().then([this](const ModeChangeRequest& req) { + log("ChangeMode requested: mode", req.mode); + controller_->request_mode_change(req.mode); + }); - on>().then([this](const LowCmdMessage& cmd) { - controller_->set_low_cmd(cmd.cmd_type, cmd.motors); - }); + on>().then( + [this](const LowCmdMessage& cmd) { controller_->set_low_cmd(cmd.cmd_type, cmd.motors); }); - // Locomotion policies (walk, get-up, lie-down, kick) moved to the NUbots_K1 side; - // they arrive as LowCmd servo targets in CUSTOM mode. The old high-level RPCs stay - // on the wire for SDK compatibility but are ignored with a warning. - on>().then([this](const WalkCommand&) { - warn_once(walk_warned_, "Move"); - }); - on>().then([this](const GetUpRequest&) { - warn_once(getup_warned_, "GetUp"); - }); - on>().then([this](const LieDownRequest&) { - warn_once(liedown_warned_, "LieDown"); - }); - on>().then([this](const VisualKickRequest&) { - warn_once(kick_warned_, "VisualKick"); - }); + // Locomotion policies (walk, get-up, lie-down, kick) moved to the NUbots_K1 side; + // they arrive as LowCmd servo targets in CUSTOM mode. The old high-level RPCs stay + // on the wire for SDK compatibility but are ignored with a warning. + on>().then([this](const WalkCommand&) { warn_once(walk_warned_, "Move"); }); + on>().then([this](const GetUpRequest&) { warn_once(getup_warned_, "GetUp"); }); + on>().then([this](const LieDownRequest&) { warn_once(liedown_warned_, "LieDown"); }); + on>().then( + [this](const VisualKickRequest&) { warn_once(kick_warned_, "VisualKick"); }); - on().then([this] { log("Locomotion shutting down"); }); -} + on().then([this] { log("Locomotion shutting down"); }); + } -void Locomotion::warn_once(bool& flag, const char* rpc) { - if (!flag) { - flag = true; - log(rpc, - "RPC received, but locomotion policies live in NUbots_K1 now; " - "ignored (drive the robot with CUSTOM mode + rt/joint_ctrl)"); + void Locomotion::warn_once(bool& flag, const char* rpc) { + if (!flag) { + flag = true; + log(rpc, + "RPC received, but locomotion policies live in NUbots_K1 now; " + "ignored (drive the robot with CUSTOM mode + rt/joint_ctrl)"); + } } -} } // namespace k1sim::module diff --git a/mujoco/module/Locomotion/src/Locomotion.hpp b/mujoco/module/Locomotion/src/Locomotion.hpp index 217428b..cb469ba 100644 --- a/mujoco/module/Locomotion/src/Locomotion.hpp +++ b/mujoco/module/Locomotion/src/Locomotion.hpp @@ -8,27 +8,27 @@ namespace k1sim::module { -// Reduced mode state machine (Damping/Prepare/Custom). Emits ControllerHandle; -// consumes HeadCommand/ModeChangeRequest/LowCmdMessage and forwards each into -// LocomotionController's thread-safe setters. Locomotion *policies* live in -// NUbots_K1 and reach the sim as LowCmd servo targets; the old high-level RPCs -// (Move/GetUp/LieDown/VisualKick) are accepted on the wire but ignored with a -// warning. The FSM logic itself lives in LocomotionController (deliberately -// NUClear-free, see that header); this reactor only wires it to NUClear and -// logs incoming requests. -class Locomotion : public NUClear::Reactor { -public: - explicit Locomotion(std::unique_ptr environment); - -private: - void warn_once(bool& flag, const char* rpc); - - std::unique_ptr controller_; - bool walk_warned_ = false; - bool getup_warned_ = false; - bool liedown_warned_ = false; - bool kick_warned_ = false; -}; + // Reduced mode state machine (Damping/Prepare/Custom). Emits ControllerHandle; + // consumes HeadCommand/ModeChangeRequest/LowCmdMessage and forwards each into + // LocomotionController's thread-safe setters. Locomotion *policies* live in + // NUbots_K1 and reach the sim as LowCmd servo targets; the old high-level RPCs + // (Move/GetUp/LieDown/VisualKick) are accepted on the wire but ignored with a + // warning. The FSM logic itself lives in LocomotionController (deliberately + // NUClear-free, see that header); this reactor only wires it to NUClear and + // logs incoming requests. + class Locomotion : public NUClear::Reactor { + public: + explicit Locomotion(std::unique_ptr environment); + + private: + void warn_once(bool& flag, const char* rpc); + + std::unique_ptr controller_; + bool walk_warned_ = false; + bool getup_warned_ = false; + bool liedown_warned_ = false; + bool kick_warned_ = false; + }; } // namespace k1sim::module diff --git a/mujoco/module/Locomotion/src/LocomotionController.cpp b/mujoco/module/Locomotion/src/LocomotionController.cpp index e58cd0b..ed3ee65 100644 --- a/mujoco/module/Locomotion/src/LocomotionController.cpp +++ b/mujoco/module/Locomotion/src/LocomotionController.cpp @@ -9,258 +9,258 @@ namespace k1sim::module { -namespace { - -// SDK-documented RotateHead limits. Only honoured in PREPARE (CUSTOM commands -// the head through LowCmd like every other joint). -constexpr double kHeadPitchMin = -0.3; -constexpr double kHeadPitchMax = 1.0; -constexpr double kHeadYawLimit = 0.785; - -void apply_head_target(std::array& q_ref, double pitch, double yaw) { - q_ref[JointIndexK1::HeadPitch] = std::clamp(pitch, kHeadPitchMin, kHeadPitchMax); - q_ref[JointIndexK1::HeadYaw] = std::clamp(yaw, -kHeadYawLimit, kHeadYawLimit); -} - -std::array load_array(const YAML::Node& node) { - std::array arr{}; - if (!node) { - return arr; + namespace { + + // SDK-documented RotateHead limits. Only honoured in PREPARE (CUSTOM commands + // the head through LowCmd like every other joint). + constexpr double kHeadPitchMin = -0.3; + constexpr double kHeadPitchMax = 1.0; + constexpr double kHeadYawLimit = 0.785; + + void apply_head_target(std::array& q_ref, double pitch, double yaw) { + q_ref[JointIndexK1::HeadPitch] = std::clamp(pitch, kHeadPitchMin, kHeadPitchMax); + q_ref[JointIndexK1::HeadYaw] = std::clamp(yaw, -kHeadYawLimit, kHeadYawLimit); + } + + std::array load_array(const YAML::Node& node) { + std::array arr{}; + if (!node) { + return arr; + } + const auto values = node.as>(); + for (std::size_t i = 0; i < JOINT_COUNT && i < values.size(); ++i) { + arr[i] = values[i]; + } + return arr; + } + + } // namespace + + LocomotionController::LocomotionController(const YAML::Node& locomotion_cfg, const YAML::Node& gains_cfg) { + prepare_blend_time_ = locomotion_cfg["prepare_blend_time"].as(1.0); + + const auto fall_node = locomotion_cfg["fall"]; + falling_tilt_ = fall_node["falling_tilt"].as(0.35); + fallen_tilt_ = fall_node["fallen_tilt"].as(1.0); + falling_gyro_ = fall_node["falling_gyro"].as(1.0); + fallen_height_ = fall_node["fallen_height"].as(0.35); + + ready_pose_ = load_array(gains_cfg["ready_pose"]); + pd_.kp = load_array(gains_cfg["kp"]); + pd_.kd = load_array(gains_cfg["kd"]); + + // The real robot boots into DAMPING (limp until an operator sends Prepare), but a + // sim robot spawns standing at the ready pose -- idling in DAMPING just collapses + // it before any client connects. Default to holding PREPARE until the first + // ChangeMode arrives. + const std::string initial_mode = locomotion_cfg["initial_mode"].as("prepare"); + if (initial_mode == "prepare") { + request_mode_change(booster::PREPARE); + } + else if (initial_mode != "damping") { + throw std::runtime_error("config/locomotion.yaml: unknown initial_mode '" + initial_mode + + "' (expected 'prepare' or 'damping')"); + } } - const auto values = node.as>(); - for (std::size_t i = 0; i < JOINT_COUNT && i < values.size(); ++i) { - arr[i] = values[i]; + + void LocomotionController::ensure_initialized(const mjModel* m) { + if (initialized_) { + return; + } + map_ = std::make_unique(ModelMap::build(m)); + initialized_ = true; } - return arr; -} - -} // namespace - -LocomotionController::LocomotionController(const YAML::Node& locomotion_cfg, const YAML::Node& gains_cfg) { - prepare_blend_time_ = locomotion_cfg["prepare_blend_time"].as(1.0); - - const auto fall_node = locomotion_cfg["fall"]; - falling_tilt_ = fall_node["falling_tilt"].as(0.35); - fallen_tilt_ = fall_node["fallen_tilt"].as(1.0); - falling_gyro_ = fall_node["falling_gyro"].as(1.0); - fallen_height_ = fall_node["fallen_height"].as(0.35); - - ready_pose_ = load_array(gains_cfg["ready_pose"]); - pd_.kp = load_array(gains_cfg["kp"]); - pd_.kd = load_array(gains_cfg["kd"]); - - // The real robot boots into DAMPING (limp until an operator sends Prepare), but a - // sim robot spawns standing at the ready pose -- idling in DAMPING just collapses - // it before any client connects. Default to holding PREPARE until the first - // ChangeMode arrives. - const std::string initial_mode = locomotion_cfg["initial_mode"].as("prepare"); - if (initial_mode == "prepare") { - request_mode_change(booster::PREPARE); + + std::array LocomotionController::current_q(mjData* d) const { + std::array q{}; + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + q[i] = d->qpos[map_->qpos_adr[i]]; + } + return q; } - else if (initial_mode != "damping") { - throw std::runtime_error("config/locomotion.yaml: unknown initial_mode '" + initial_mode - + "' (expected 'prepare' or 'damping')"); + + LocomotionController::CommandSnapshot LocomotionController::snapshot_command() const { + std::lock_guard lock(cmd_mutex_); + CommandSnapshot snap; + snap.head_pitch = head_pitch_; + snap.head_yaw = head_yaw_; + snap.mode_seq = mode_seq_; + snap.requested_mode = requested_mode_; + snap.low_cmd_type = low_cmd_type_; + snap.low_cmd_motors = low_cmd_motors_; + return snap; } -} -void LocomotionController::ensure_initialized(const mjModel* m) { - if (initialized_) { - return; + void LocomotionController::set_head_command(double pitch, double yaw) { + std::lock_guard lock(cmd_mutex_); + head_pitch_ = pitch; + head_yaw_ = yaw; } - map_ = std::make_unique(ModelMap::build(m)); - initialized_ = true; -} - -std::array LocomotionController::current_q(mjData* d) const { - std::array q{}; - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - q[i] = d->qpos[map_->qpos_adr[i]]; + + void LocomotionController::request_mode_change(int mode) { + std::lock_guard lock(cmd_mutex_); + requested_mode_ = mode; + ++mode_seq_; } - return q; -} - -LocomotionController::CommandSnapshot LocomotionController::snapshot_command() const { - std::lock_guard lock(cmd_mutex_); - CommandSnapshot snap; - snap.head_pitch = head_pitch_; - snap.head_yaw = head_yaw_; - snap.mode_seq = mode_seq_; - snap.requested_mode = requested_mode_; - snap.low_cmd_type = low_cmd_type_; - snap.low_cmd_motors = low_cmd_motors_; - return snap; -} - -void LocomotionController::set_head_command(double pitch, double yaw) { - std::lock_guard lock(cmd_mutex_); - head_pitch_ = pitch; - head_yaw_ = yaw; -} - -void LocomotionController::request_mode_change(int mode) { - std::lock_guard lock(cmd_mutex_); - requested_mode_ = mode; - ++mode_seq_; -} - -void LocomotionController::set_low_cmd(int cmd_type, std::vector motors) { - std::lock_guard lock(cmd_mutex_); - low_cmd_type_ = cmd_type; - low_cmd_motors_ = std::move(motors); -} - -void LocomotionController::step(const mjModel* m, mjData* d) { - ensure_initialized(m); - const CommandSnapshot snap = snapshot_command(); - - if (snap.mode_seq != last_mode_seq_) { - last_mode_seq_ = snap.mode_seq; - handle_mode_request(snap.requested_mode, d); + + void LocomotionController::set_low_cmd(int cmd_type, std::vector motors) { + std::lock_guard lock(cmd_mutex_); + low_cmd_type_ = cmd_type; + low_cmd_motors_ = std::move(motors); } - update_fall_detection(m, d); + void LocomotionController::step(const mjModel* m, mjData* d) { + ensure_initialized(m); + const CommandSnapshot snap = snapshot_command(); - switch (state_) { - case State::Damping: step_damping(d); break; - case State::Prepare: step_prepare(m, d, snap); break; - case State::Custom: step_custom(m, d, snap); break; - } -} - -void LocomotionController::handle_mode_request(int requested_mode, mjData* d) { - int effective = requested_mode; - switch (requested_mode) { - case booster::DAMPING: - case booster::PREPARE: - case booster::CUSTOM: break; - case booster::WALKING: - case booster::SOCCER: - // Wire-compatible, but the walking policy lives on the NUbots_K1 side now - // (rt/joint_ctrl in CUSTOM mode). Hold the ready pose instead. - std::fprintf(stderr, - "LocomotionController: ChangeMode(%d) requested, but locomotion policies " - "live in NUbots_K1 now; holding PREPARE (use CUSTOM + rt/joint_ctrl)\n", - requested_mode); - effective = booster::PREPARE; - break; - default: - std::fprintf(stderr, "LocomotionController: ignoring unknown ChangeMode value %d\n", requested_mode); - return; - } + if (snap.mode_seq != last_mode_seq_) { + last_mode_seq_ = snap.mode_seq; + handle_mode_request(snap.requested_mode, d); + } - mode_.store(effective, std::memory_order_relaxed); - switch (effective) { - case booster::DAMPING: state_ = State::Damping; break; - case booster::PREPARE: - state_ = State::Prepare; - prepare_start_pose_ = current_q(d); - prepare_start_time_ = d->time; - break; - case booster::CUSTOM: - state_ = State::Custom; - custom_parallel_warned_ = false; - // PD-hold the pose we entered CUSTOM in until the first LowCmd arrives; - // freezing raw torques instead lets the robot slump if the client dies - // (or never manages to stream) after switching modes. Drop any LowCmd left - // over from a previous CUSTOM session for the same reason. - custom_entry_pose_ = current_q(d); - custom_low_cmd_seen_ = false; - custom_entry_step_ = true; - { - std::lock_guard lock(cmd_mutex_); - low_cmd_motors_.clear(); - } - break; - default: break; // unreachable - } -} + update_fall_detection(m, d); -void LocomotionController::step_damping(mjData* d) { - for (int act : map_->act_id) { - d->ctrl[act] = 0.0; + switch (state_) { + case State::Damping: step_damping(d); break; + case State::Prepare: step_prepare(m, d, snap); break; + case State::Custom: step_custom(m, d, snap); break; + } } -} -void LocomotionController::step_prepare(const mjModel* m, mjData* d, const CommandSnapshot& snap) { - const double frac = std::clamp((d->time - prepare_start_time_) / prepare_blend_time_, 0.0, 1.0); - const double s = smoothstep(frac); + void LocomotionController::handle_mode_request(int requested_mode, mjData* d) { + int effective = requested_mode; + switch (requested_mode) { + case booster::DAMPING: + case booster::PREPARE: + case booster::CUSTOM: break; + case booster::WALKING: + case booster::SOCCER: + // Wire-compatible, but the walking policy lives on the NUbots_K1 side now + // (rt/joint_ctrl in CUSTOM mode). Hold the ready pose instead. + std::fprintf(stderr, + "LocomotionController: ChangeMode(%d) requested, but locomotion policies " + "live in NUbots_K1 now; holding PREPARE (use CUSTOM + rt/joint_ctrl)\n", + requested_mode); + effective = booster::PREPARE; + break; + default: + std::fprintf(stderr, "LocomotionController: ignoring unknown ChangeMode value %d\n", requested_mode); + return; + } - std::array q_ref{}; - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - q_ref[i] = lerp(prepare_start_pose_[i], ready_pose_[i], s); + mode_.store(effective, std::memory_order_relaxed); + switch (effective) { + case booster::DAMPING: state_ = State::Damping; break; + case booster::PREPARE: + state_ = State::Prepare; + prepare_start_pose_ = current_q(d); + prepare_start_time_ = d->time; + break; + case booster::CUSTOM: + state_ = State::Custom; + custom_parallel_warned_ = false; + // PD-hold the pose we entered CUSTOM in until the first LowCmd arrives; + // freezing raw torques instead lets the robot slump if the client dies + // (or never manages to stream) after switching modes. Drop any LowCmd left + // over from a previous CUSTOM session for the same reason. + custom_entry_pose_ = current_q(d); + custom_low_cmd_seen_ = false; + custom_entry_step_ = true; + { + std::lock_guard lock(cmd_mutex_); + low_cmd_motors_.clear(); + } + break; + default: break; // unreachable + } } - apply_head_target(q_ref, snap.head_pitch, snap.head_yaw); - pd_.apply(m, d, *map_, q_ref); -} - -void LocomotionController::step_custom(const mjModel* m, mjData* d, const CommandSnapshot& snap) { - // Until the first LowCmd of this CUSTOM session arrives, PD-hold the entry pose - // (a dead or struggling client must not leave the robot on frozen torques). The - // entry step's snapshot predates the mailbox clear, so it is skipped explicitly. - if (custom_entry_step_) { - custom_entry_step_ = false; - pd_.apply(m, d, *map_, custom_entry_pose_); - return; + + void LocomotionController::step_damping(mjData* d) { + for (int act : map_->act_id) { + d->ctrl[act] = 0.0; + } } - if (!custom_low_cmd_seen_) { - if (!snap.low_cmd_motors.empty()) { - custom_low_cmd_seen_ = true; + + void LocomotionController::step_prepare(const mjModel* m, mjData* d, const CommandSnapshot& snap) { + const double frac = std::clamp((d->time - prepare_start_time_) / prepare_blend_time_, 0.0, 1.0); + const double s = smoothstep(frac); + + std::array q_ref{}; + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + q_ref[i] = lerp(prepare_start_pose_[i], ready_pose_[i], s); } - else { + apply_head_target(q_ref, snap.head_pitch, snap.head_yaw); + pd_.apply(m, d, *map_, q_ref); + } + + void LocomotionController::step_custom(const mjModel* m, mjData* d, const CommandSnapshot& snap) { + // Until the first LowCmd of this CUSTOM session arrives, PD-hold the entry pose + // (a dead or struggling client must not leave the robot on frozen torques). The + // entry step's snapshot predates the mailbox clear, so it is skipped explicitly. + if (custom_entry_step_) { + custom_entry_step_ = false; pd_.apply(m, d, *map_, custom_entry_pose_); return; } - } + if (!custom_low_cmd_seen_) { + if (!snap.low_cmd_motors.empty()) { + custom_low_cmd_seen_ = true; + } + else { + pd_.apply(m, d, *map_, custom_entry_pose_); + return; + } + } - if (snap.low_cmd_type == 0) { // PARALLEL: unsupported, hold last ctrl - if (!custom_parallel_warned_) { - std::fprintf(stderr, - "LocomotionController: CUSTOM received cmd_type=PARALLEL, which is not " - "supported; holding last ctrl\n"); - custom_parallel_warned_ = true; + if (snap.low_cmd_type == 0) { // PARALLEL: unsupported, hold last ctrl + if (!custom_parallel_warned_) { + std::fprintf(stderr, + "LocomotionController: CUSTOM received cmd_type=PARALLEL, which is not " + "supported; holding last ctrl\n"); + custom_parallel_warned_ = true; + } + return; } - return; - } - const auto& motors = snap.low_cmd_motors; - const std::size_t n = std::min(motors.size(), JOINT_COUNT); - for (std::size_t i = 0; i < n; ++i) { - const auto& mc = motors[i]; - const int act = map_->act_id[i]; - const double q = d->qpos[map_->qpos_adr[i]]; - const double dq = d->qvel[map_->dof_adr[i]]; - double tau = mc.kp * (mc.q - q) + mc.kd * (mc.dq - dq) + mc.tau; - if (m->actuator_forcelimited[act] != 0) { - tau = std::clamp(tau, m->actuator_forcerange[2 * act], m->actuator_forcerange[2 * act + 1]); + const auto& motors = snap.low_cmd_motors; + const std::size_t n = std::min(motors.size(), JOINT_COUNT); + for (std::size_t i = 0; i < n; ++i) { + const auto& mc = motors[i]; + const int act = map_->act_id[i]; + const double q = d->qpos[map_->qpos_adr[i]]; + const double dq = d->qvel[map_->dof_adr[i]]; + double tau = mc.kp * (mc.q - q) + mc.kd * (mc.dq - dq) + mc.tau; + if (m->actuator_forcelimited[act] != 0) { + tau = std::clamp(tau, m->actuator_forcerange[2 * act], m->actuator_forcerange[2 * act + 1]); + } + d->ctrl[act] = tau; } - d->ctrl[act] = tau; - } - // Joints beyond `n` (an undersized LowCmd) are left at their previous - // ctrl value -- the same "hold" behaviour as a PARALLEL command. -} - -void LocomotionController::update_fall_detection(const mjModel* /*m*/, mjData* d) { - const double tilt = base_tilt(d, *map_); - double gyro_mag = 0.0; - if (map_->sens_gyro >= 0) { - const double* g = &d->sensordata[map_->sens_gyro]; - gyro_mag = std::sqrt(g[0] * g[0] + g[1] * g[1] + g[2] * g[2]); + // Joints beyond `n` (an undersized LowCmd) are left at their previous + // ctrl value -- the same "hold" behaviour as a PARALLEL command. } - // Height criterion catches collapsed-but-trunk-upright poses (kneeling crumples - // after a failed get-up) that tilt alone reads as IS_READY -- which starves the - // NUbots GetUp retry loop forever. - const double base_z = d->qpos[map_->root_qpos_adr + 2]; + void LocomotionController::update_fall_detection(const mjModel* /*m*/, mjData* d) { + const double tilt = base_tilt(d, *map_); + double gyro_mag = 0.0; + if (map_->sens_gyro >= 0) { + const double* g = &d->sensordata[map_->sens_gyro]; + gyro_mag = std::sqrt(g[0] * g[0] + g[1] * g[1] + g[2] * g[2]); + } - int computed = booster::IS_READY; - if (tilt > fallen_tilt_ || base_z < fallen_height_) { - computed = booster::HAS_FALLEN; - } - else if (tilt > falling_tilt_ && gyro_mag > falling_gyro_) { - computed = booster::IS_FALLING; - } + // Height criterion catches collapsed-but-trunk-upright poses (kneeling crumples + // after a failed get-up) that tilt alone reads as IS_READY -- which starves the + // NUbots GetUp retry loop forever. + const double base_z = d->qpos[map_->root_qpos_adr + 2]; + + int computed = booster::IS_READY; + if (tilt > fallen_tilt_ || base_z < fallen_height_) { + computed = booster::HAS_FALLEN; + } + else if (tilt > falling_tilt_ && gyro_mag > falling_gyro_) { + computed = booster::IS_FALLING; + } - fall_state_.store(computed, std::memory_order_relaxed); -} + fall_state_.store(computed, std::memory_order_relaxed); + } } // namespace k1sim::module diff --git a/mujoco/module/Locomotion/src/LocomotionController.hpp b/mujoco/module/Locomotion/src/LocomotionController.hpp index 3088057..c6a1259 100644 --- a/mujoco/module/Locomotion/src/LocomotionController.hpp +++ b/mujoco/module/Locomotion/src/LocomotionController.hpp @@ -25,114 +25,114 @@ namespace k1sim::module { -// The reduced Booster mode state machine: DAMPING (motors limp), PREPARE -// (blend to and hold the gains.yaml ready pose -- boot convenience, so the -// robot doesn't collapse before a client connects) and CUSTOM (PD-track the -// rt/joint_ctrl LowCmd servo targets). All locomotion *policies* (walking, -// get-up, kick, ...) live on the NUbots_K1 side and arrive here as LowCmd -// joint commands -- the sim only simulates servos and publishes state. -// WALKING/SOCCER mode requests are accepted for SDK wire compatibility but -// mapped to PREPARE with a warning. -// -// Thread model: setters below are called from arbitrary NUClear reaction -// threads (SdkBridge's RPC dispatch) and only ever write a mutex-guarded -// mailbox. step() runs on the physics thread with the sim mutex already held -// (per StepController's contract) and snapshots the mailbox once per call; -// all FSM state is touched only from step(). mode()/fall_state() are the only -// cross-thread reads and are plain atomics. -class LocomotionController : public StepController { -public: - // locomotion_cfg: config/locomotion.yaml root node. - // gains_cfg: config/gains.yaml root node (kp/kd/ready_pose, JointIndexK1 order). - LocomotionController(const YAML::Node& locomotion_cfg, const YAML::Node& gains_cfg); - - // -- StepController -- - void step(const mjModel* m, mjData* d) override; - int mode() const override { - return mode_.load(std::memory_order_relaxed); - } - int fall_state() const override { - return fall_state_.load(std::memory_order_relaxed); - } - bool getting_up() const override { - return false; // scripted get-up removed; recovery is a NUbots_K1 policy - } - - // -- thread-safe setters (called from NUClear reaction threads) -- - void set_head_command(double pitch, double yaw); - void request_mode_change(int mode); - void set_low_cmd(int cmd_type, std::vector motors); - - // Test/introspection helper (harmless in production: read-only). - bool is_initialized() const { - return initialized_; - } - -private: - enum class State { Damping, Prepare, Custom }; - - // Mailbox written by the setters above, read once per step() via a copy - // taken under lock. `mode_seq` is an edge-trigger: step() remembers the - // last value it consumed and acts only when the sequence has advanced. - struct CommandSnapshot { - double head_pitch = 0, head_yaw = 0; - uint64_t mode_seq = 0; - int requested_mode = booster::DAMPING; - int low_cmd_type = 1; - std::vector low_cmd_motors; + // The reduced Booster mode state machine: DAMPING (motors limp), PREPARE + // (blend to and hold the gains.yaml ready pose -- boot convenience, so the + // robot doesn't collapse before a client connects) and CUSTOM (PD-track the + // rt/joint_ctrl LowCmd servo targets). All locomotion *policies* (walking, + // get-up, kick, ...) live on the NUbots_K1 side and arrive here as LowCmd + // joint commands -- the sim only simulates servos and publishes state. + // WALKING/SOCCER mode requests are accepted for SDK wire compatibility but + // mapped to PREPARE with a warning. + // + // Thread model: setters below are called from arbitrary NUClear reaction + // threads (SdkBridge's RPC dispatch) and only ever write a mutex-guarded + // mailbox. step() runs on the physics thread with the sim mutex already held + // (per StepController's contract) and snapshots the mailbox once per call; + // all FSM state is touched only from step(). mode()/fall_state() are the only + // cross-thread reads and are plain atomics. + class LocomotionController : public StepController { + public: + // locomotion_cfg: config/locomotion.yaml root node. + // gains_cfg: config/gains.yaml root node (kp/kd/ready_pose, JointIndexK1 order). + LocomotionController(const YAML::Node& locomotion_cfg, const YAML::Node& gains_cfg); + + // -- StepController -- + void step(const mjModel* m, mjData* d) override; + int mode() const override { + return mode_.load(std::memory_order_relaxed); + } + int fall_state() const override { + return fall_state_.load(std::memory_order_relaxed); + } + bool getting_up() const override { + return false; // scripted get-up removed; recovery is a NUbots_K1 policy + } + + // -- thread-safe setters (called from NUClear reaction threads) -- + void set_head_command(double pitch, double yaw); + void request_mode_change(int mode); + void set_low_cmd(int cmd_type, std::vector motors); + + // Test/introspection helper (harmless in production: read-only). + bool is_initialized() const { + return initialized_; + } + + private: + enum class State { Damping, Prepare, Custom }; + + // Mailbox written by the setters above, read once per step() via a copy + // taken under lock. `mode_seq` is an edge-trigger: step() remembers the + // last value it consumed and acts only when the sequence has advanced. + struct CommandSnapshot { + double head_pitch = 0, head_yaw = 0; + uint64_t mode_seq = 0; + int requested_mode = booster::DAMPING; + int low_cmd_type = 1; + std::vector low_cmd_motors; + }; + + CommandSnapshot snapshot_command() const; + + void ensure_initialized(const mjModel* m); + std::array current_q(mjData* d) const; + + void handle_mode_request(int requested_mode, mjData* d); + + void step_damping(mjData* d); + void step_prepare(const mjModel* m, mjData* d, const CommandSnapshot& snap); + void step_custom(const mjModel* m, mjData* d, const CommandSnapshot& snap); + + void update_fall_detection(const mjModel* m, mjData* d); + + // -- config (parsed once at construction; no model needed) -- + double prepare_blend_time_; + double falling_tilt_, fallen_tilt_, falling_gyro_, fallen_height_; + + std::array ready_pose_{}; + PdController pd_; + + // -- lazily built on first step() (needs the mjModel) -- + bool initialized_ = false; + std::unique_ptr map_; + + // -- FSM state (physics-thread only) -- + State state_ = State::Damping; + + std::array prepare_start_pose_{}; + double prepare_start_time_ = 0.0; + + bool custom_parallel_warned_ = false; + // PD-hold target for a CUSTOM session that has not received a LowCmd yet + std::array custom_entry_pose_{}; + bool custom_low_cmd_seen_ = false; + bool custom_entry_step_ = false; // the entry step's snapshot predates the mailbox clear + + uint64_t last_mode_seq_ = 0; + + // -- mailbox (guarded by cmd_mutex_) -- + mutable std::mutex cmd_mutex_; + double head_pitch_ = 0, head_yaw_ = 0; + uint64_t mode_seq_ = 0; + int requested_mode_ = booster::DAMPING; + int low_cmd_type_ = 1; + std::vector low_cmd_motors_; + + // -- cross-thread-visible state -- + std::atomic mode_{booster::DAMPING}; + std::atomic fall_state_{booster::IS_READY}; }; - CommandSnapshot snapshot_command() const; - - void ensure_initialized(const mjModel* m); - std::array current_q(mjData* d) const; - - void handle_mode_request(int requested_mode, mjData* d); - - void step_damping(mjData* d); - void step_prepare(const mjModel* m, mjData* d, const CommandSnapshot& snap); - void step_custom(const mjModel* m, mjData* d, const CommandSnapshot& snap); - - void update_fall_detection(const mjModel* m, mjData* d); - - // -- config (parsed once at construction; no model needed) -- - double prepare_blend_time_; - double falling_tilt_, fallen_tilt_, falling_gyro_, fallen_height_; - - std::array ready_pose_{}; - PdController pd_; - - // -- lazily built on first step() (needs the mjModel) -- - bool initialized_ = false; - std::unique_ptr map_; - - // -- FSM state (physics-thread only) -- - State state_ = State::Damping; - - std::array prepare_start_pose_{}; - double prepare_start_time_ = 0.0; - - bool custom_parallel_warned_ = false; - // PD-hold target for a CUSTOM session that has not received a LowCmd yet - std::array custom_entry_pose_{}; - bool custom_low_cmd_seen_ = false; - bool custom_entry_step_ = false; // the entry step's snapshot predates the mailbox clear - - uint64_t last_mode_seq_ = 0; - - // -- mailbox (guarded by cmd_mutex_) -- - mutable std::mutex cmd_mutex_; - double head_pitch_ = 0, head_yaw_ = 0; - uint64_t mode_seq_ = 0; - int requested_mode_ = booster::DAMPING; - int low_cmd_type_ = 1; - std::vector low_cmd_motors_; - - // -- cross-thread-visible state -- - std::atomic mode_{booster::DAMPING}; - std::atomic fall_state_{booster::IS_READY}; -}; - } // namespace k1sim::module #endif // K1SIM_MODULE_LOCOMOTION_LOCOMOTIONCONTROLLER_HPP diff --git a/mujoco/module/SdkBridge/CMakeLists.txt b/mujoco/module/SdkBridge/CMakeLists.txt index 9ff18da..b1b06ae 100644 --- a/mujoco/module/SdkBridge/CMakeLists.txt +++ b/mujoco/module/SdkBridge/CMakeLists.txt @@ -2,46 +2,35 @@ # Clean-room IDL for the Booster SDK wire types; see PROTOCOL.md for provenance. set(K1SIM_IDL_GEN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../idl_gen) -file(GLOB k1sim_idl_srcs CONFIGURE_DEPENDS - ${K1SIM_IDL_GEN_DIR}/booster_interface/*.cxx - ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.cxx - ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.cxx +file(GLOB k1sim_idl_srcs CONFIGURE_DEPENDS ${K1SIM_IDL_GEN_DIR}/booster_interface/*.cxx + ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.cxx ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.cxx ) -file(GLOB k1sim_idl_ipp CONFIGURE_DEPENDS - ${K1SIM_IDL_GEN_DIR}/booster_interface/*.ipp - ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.ipp - ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.ipp +file(GLOB k1sim_idl_ipp CONFIGURE_DEPENDS ${K1SIM_IDL_GEN_DIR}/booster_interface/*.ipp + ${K1SIM_IDL_GEN_DIR}/booster_msgs/*.ipp ${K1SIM_IDL_GEN_DIR}/geometry_msgs/*.ipp ) # Each CdrAux.ipp holds explicit fastcdr template specializations -# (calculate_serialized_size/serialize/deserialize/serialize_key for that exact -# type) that PubSubTypes.cxx calls but neither declares nor includes the -# definition of — fastddsgen expects them compiled as their own translation unit. -# CMake doesn't recognise ".ipp" as a C++ source extension by default, so mark it -# explicitly (each file's include guard + single glob-inclusion keeps this to -# exactly one definition per type across the whole link). +# (calculate_serialized_size/serialize/deserialize/serialize_key for that exact type) that PubSubTypes.cxx calls +# but neither declares nor includes the definition of — fastddsgen expects them compiled as their own translation unit. +# CMake doesn't recognise ".ipp" as a C++ source extension by default, so mark it explicitly (each file's include guard +# + single glob-inclusion keeps this to exactly one definition per type across the whole link). set_source_files_properties(${k1sim_idl_ipp} PROPERTIES LANGUAGE CXX) add_library(k1sim_idl STATIC ${k1sim_idl_srcs} ${k1sim_idl_ipp}) target_include_directories( - k1sim_idl - PUBLIC ${K1SIM_IDL_GEN_DIR}/booster_interface ${K1SIM_IDL_GEN_DIR}/booster_msgs - ${K1SIM_IDL_GEN_DIR}/geometry_msgs + k1sim_idl PUBLIC ${K1SIM_IDL_GEN_DIR}/booster_interface ${K1SIM_IDL_GEN_DIR}/booster_msgs + ${K1SIM_IDL_GEN_DIR}/geometry_msgs ) target_link_libraries(k1sim_idl PUBLIC fastrtps fastcdr) # --- module::SdkBridge --- add_library( - k1sim_module_sdkbridge STATIC - src/SdkBridge.cpp - src/DdsParticipant.cpp - src/StatePublisher.cpp - src/RpcServer.cpp - src/RpcDispatch.cpp + k1sim_module_sdkbridge STATIC src/DdsParticipant.cpp src/RpcDispatch.cpp src/RpcServer.cpp src/SdkBridge.cpp + src/StatePublisher.cpp ) target_link_libraries( - k1sim_module_sdkbridge - PUBLIC k1sim_shared k1sim_idl - PRIVATE fastrtps fastcdr nlohmann_json::nlohmann_json + k1sim_module_sdkbridge + PUBLIC k1sim_shared k1sim_idl + PRIVATE fastrtps fastcdr nlohmann_json::nlohmann_json ) add_subdirectory(test_support) diff --git a/mujoco/module/SdkBridge/src/DdsParticipant.cpp b/mujoco/module/SdkBridge/src/DdsParticipant.cpp index 184fb40..c6fa997 100644 --- a/mujoco/module/SdkBridge/src/DdsParticipant.cpp +++ b/mujoco/module/SdkBridge/src/DdsParticipant.cpp @@ -10,67 +10,67 @@ namespace k1sim::module::sdkbridge { -using namespace eprosima::fastdds::dds; // NOLINT — matches Fast-DDS's own usage idiom + using namespace eprosima::fastdds::dds; // NOLINT — matches Fast-DDS's own usage idiom -DdsParticipant::DdsParticipant(int domain_id, bool udp_only) { - DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT; - pqos.name("k1sim_sdkbridge"); - pqos.setup_transports(udp_only ? eprosima::fastdds::rtps::BuiltinTransports::UDPv4 - : eprosima::fastdds::rtps::BuiltinTransports::DEFAULT); + DdsParticipant::DdsParticipant(int domain_id, bool udp_only) { + DomainParticipantQos pqos = PARTICIPANT_QOS_DEFAULT; + pqos.name("k1sim_sdkbridge"); + pqos.setup_transports(udp_only ? eprosima::fastdds::rtps::BuiltinTransports::UDPv4 + : eprosima::fastdds::rtps::BuiltinTransports::DEFAULT); - participant_ = DomainParticipantFactory::get_instance()->create_participant(domain_id, pqos); - if (participant_ == nullptr) { - throw std::runtime_error("DdsParticipant: failed to create DomainParticipant on domain " - + std::to_string(domain_id)); - } + participant_ = DomainParticipantFactory::get_instance()->create_participant(domain_id, pqos); + if (participant_ == nullptr) { + throw std::runtime_error("DdsParticipant: failed to create DomainParticipant on domain " + + std::to_string(domain_id)); + } - publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT); - if (publisher_ == nullptr) { - throw std::runtime_error("DdsParticipant: failed to create Publisher"); - } + publisher_ = participant_->create_publisher(PUBLISHER_QOS_DEFAULT); + if (publisher_ == nullptr) { + throw std::runtime_error("DdsParticipant: failed to create Publisher"); + } - subscriber_ = participant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT); - if (subscriber_ == nullptr) { - throw std::runtime_error("DdsParticipant: failed to create Subscriber"); + subscriber_ = participant_->create_subscriber(SUBSCRIBER_QOS_DEFAULT); + if (subscriber_ == nullptr) { + throw std::runtime_error("DdsParticipant: failed to create Subscriber"); + } } -} -DdsParticipant::~DdsParticipant() { - if (participant_ != nullptr) { - participant_->delete_contained_entities(); - DomainParticipantFactory::get_instance()->delete_participant(participant_); + DdsParticipant::~DdsParticipant() { + if (participant_ != nullptr) { + participant_->delete_contained_entities(); + DomainParticipantFactory::get_instance()->delete_participant(participant_); + } } -} - -DataWriterQos DdsParticipant::state_writer_qos(int depth) { - DataWriterQos qos = DATAWRITER_QOS_DEFAULT; - qos.reliability().kind = RELIABLE_RELIABILITY_QOS; - qos.durability().kind = VOLATILE_DURABILITY_QOS; - qos.history().kind = KEEP_LAST_HISTORY_QOS; - qos.history().depth = depth; - return qos; -} -DataReaderQos DdsParticipant::rpc_request_reader_qos(int depth) { - DataReaderQos qos = DATAREADER_QOS_DEFAULT; - qos.reliability().kind = RELIABLE_RELIABILITY_QOS; - qos.history().kind = KEEP_LAST_HISTORY_QOS; - qos.history().depth = depth; - return qos; -} + DataWriterQos DdsParticipant::state_writer_qos(int depth) { + DataWriterQos qos = DATAWRITER_QOS_DEFAULT; + qos.reliability().kind = RELIABLE_RELIABILITY_QOS; + qos.durability().kind = VOLATILE_DURABILITY_QOS; + qos.history().kind = KEEP_LAST_HISTORY_QOS; + qos.history().depth = depth; + return qos; + } -Topic* DdsParticipant::get_or_create_topic(const std::string& topic_name, TypeSupport& type) { - auto it = topics_.find(topic_name); - if (it != topics_.end()) { - return it->second; + DataReaderQos DdsParticipant::rpc_request_reader_qos(int depth) { + DataReaderQos qos = DATAREADER_QOS_DEFAULT; + qos.reliability().kind = RELIABLE_RELIABILITY_QOS; + qos.history().kind = KEEP_LAST_HISTORY_QOS; + qos.history().depth = depth; + return qos; } - type.register_type(participant_); - Topic* topic = participant_->create_topic(topic_name, type.get_type_name(), TOPIC_QOS_DEFAULT); - if (topic == nullptr) { - throw std::runtime_error("DdsParticipant: failed to create topic " + topic_name); + + Topic* DdsParticipant::get_or_create_topic(const std::string& topic_name, TypeSupport& type) { + auto it = topics_.find(topic_name); + if (it != topics_.end()) { + return it->second; + } + type.register_type(participant_); + Topic* topic = participant_->create_topic(topic_name, type.get_type_name(), TOPIC_QOS_DEFAULT); + if (topic == nullptr) { + throw std::runtime_error("DdsParticipant: failed to create topic " + topic_name); + } + topics_[topic_name] = topic; + return topic; } - topics_[topic_name] = topic; - return topic; -} } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/DdsParticipant.hpp b/mujoco/module/SdkBridge/src/DdsParticipant.hpp index 31a59d0..3053971 100644 --- a/mujoco/module/SdkBridge/src/DdsParticipant.hpp +++ b/mujoco/module/SdkBridge/src/DdsParticipant.hpp @@ -22,54 +22,56 @@ namespace k1sim::module::sdkbridge { -class DdsParticipant { -public: - // udp_only strips the SHM transport (config/dds.yaml: udp_only, or env - // K1_DDS_UDP_ONLY=1), leaving UDPv4 only — a workaround for docker boundaries - // where /dev/shm isn't shared (e.g. no --ipc host). - DdsParticipant(int domain_id, bool udp_only); - ~DdsParticipant(); + class DdsParticipant { + public: + // udp_only strips the SHM transport (config/dds.yaml: udp_only, or env + // K1_DDS_UDP_ONLY=1), leaving UDPv4 only — a workaround for docker boundaries + // where /dev/shm isn't shared (e.g. no --ipc host). + DdsParticipant(int domain_id, bool udp_only); + ~DdsParticipant(); - DdsParticipant(const DdsParticipant&) = delete; - DdsParticipant& operator=(const DdsParticipant&) = delete; + DdsParticipant(const DdsParticipant&) = delete; + DdsParticipant& operator=(const DdsParticipant&) = delete; - // PROTOCOL.md §4: state writers use RELIABLE + VOLATILE + KEEP_LAST(depth). - static eprosima::fastdds::dds::DataWriterQos state_writer_qos(int depth = 5); - // PROTOCOL.md §4: the RPC request reader uses RELIABLE + KEEP_LAST(depth) so a - // burst of calls at startup (e.g. NUbots' immediate ChangeMode) isn't dropped. - static eprosima::fastdds::dds::DataReaderQos rpc_request_reader_qos(int depth = 10); + // PROTOCOL.md §4: state writers use RELIABLE + VOLATILE + KEEP_LAST(depth). + static eprosima::fastdds::dds::DataWriterQos state_writer_qos(int depth = 5); + // PROTOCOL.md §4: the RPC request reader uses RELIABLE + KEEP_LAST(depth) so a + // burst of calls at startup (e.g. NUbots' immediate ChangeMode) isn't dropped. + static eprosima::fastdds::dds::DataReaderQos rpc_request_reader_qos(int depth = 10); - // PubSubTypeT is a generated `_PubSubType` (e.g. - // booster_interface::msg::dds_::Odometer_PubSubType). Registers the type (once - // per topic_name) and creates the writer/reader on it. - template - eprosima::fastdds::dds::DataWriter* create_writer(const std::string& topic_name, - const eprosima::fastdds::dds::DataWriterQos& qos) { - eprosima::fastdds::dds::TypeSupport type(new PubSubTypeT()); - auto* topic = get_or_create_topic(topic_name, type); - return publisher_->create_datawriter(topic, qos); - } + // PubSubTypeT is a generated `_PubSubType` (e.g. + // booster_interface::msg::dds_::Odometer_PubSubType). Registers the type (once + // per topic_name) and creates the writer/reader on it. + template + eprosima::fastdds::dds::DataWriter* create_writer(const std::string& topic_name, + const eprosima::fastdds::dds::DataWriterQos& qos) { + eprosima::fastdds::dds::TypeSupport type(new PubSubTypeT()); + auto* topic = get_or_create_topic(topic_name, type); + return publisher_->create_datawriter(topic, qos); + } - template - eprosima::fastdds::dds::DataReader* create_reader(const std::string& topic_name, - const eprosima::fastdds::dds::DataReaderQos& qos, - eprosima::fastdds::dds::DataReaderListener* listener) { - eprosima::fastdds::dds::TypeSupport type(new PubSubTypeT()); - auto* topic = get_or_create_topic(topic_name, type); - return subscriber_->create_datareader(topic, qos, listener); - } + template + eprosima::fastdds::dds::DataReader* create_reader(const std::string& topic_name, + const eprosima::fastdds::dds::DataReaderQos& qos, + eprosima::fastdds::dds::DataReaderListener* listener) { + eprosima::fastdds::dds::TypeSupport type(new PubSubTypeT()); + auto* topic = get_or_create_topic(topic_name, type); + return subscriber_->create_datareader(topic, qos, listener); + } - eprosima::fastdds::dds::DomainParticipant* participant() const { return participant_; } + eprosima::fastdds::dds::DomainParticipant* participant() const { + return participant_; + } -private: - eprosima::fastdds::dds::Topic* get_or_create_topic(const std::string& topic_name, - eprosima::fastdds::dds::TypeSupport& type); + private: + eprosima::fastdds::dds::Topic* get_or_create_topic(const std::string& topic_name, + eprosima::fastdds::dds::TypeSupport& type); - eprosima::fastdds::dds::DomainParticipant* participant_ = nullptr; - eprosima::fastdds::dds::Publisher* publisher_ = nullptr; - eprosima::fastdds::dds::Subscriber* subscriber_ = nullptr; - std::map topics_; -}; + eprosima::fastdds::dds::DomainParticipant* participant_ = nullptr; + eprosima::fastdds::dds::Publisher* publisher_ = nullptr; + eprosima::fastdds::dds::Subscriber* subscriber_ = nullptr; + std::map topics_; + }; } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/RpcDispatch.cpp b/mujoco/module/SdkBridge/src/RpcDispatch.cpp index abc10a8..d61a77c 100644 --- a/mujoco/module/SdkBridge/src/RpcDispatch.cpp +++ b/mujoco/module/SdkBridge/src/RpcDispatch.cpp @@ -7,114 +7,114 @@ namespace k1sim::module::sdkbridge { -namespace { + namespace { -using nlohmann::json; + using nlohmann::json; -// RpcReqMsg_.body is sometimes a genuinely empty string (LIE_DOWN, GET_UP, GET_MODE -// requests carry no parameters) — nlohmann::json::parse("") throws, so treat blank -// (or whitespace-only) bodies as an empty object rather than a parse error. -json parse_or_empty(const std::string& text) { - auto trimmed_empty = [&] { - for (char c : text) { - if (!std::isspace(static_cast(c))) { - return false; + // RpcReqMsg_.body is sometimes a genuinely empty string (LIE_DOWN, GET_UP, GET_MODE + // requests carry no parameters) — nlohmann::json::parse("") throws, so treat blank + // (or whitespace-only) bodies as an empty object rather than a parse error. + json parse_or_empty(const std::string& text) { + auto trimmed_empty = [&] { + for (char c : text) { + if (!std::isspace(static_cast(c))) { + return false; + } + } + return true; + }(); + if (trimmed_empty) { + return json::object(); + } + try { + return json::parse(text); + } + catch (const json::parse_error&) { + return json::object(); } } - return true; - }(); - if (trimmed_empty) { - return json::object(); - } - try { - return json::parse(text); - } - catch (const json::parse_error&) { - return json::object(); - } -} - -} // namespace -RpcOutcome dispatch_rpc(const std::string& header_json, - const std::string& body_json, - int current_mode, - int64_t unknown_api_status) { - RpcOutcome outcome; - - const json header = parse_or_empty(header_json); - if (!header.contains("api_id")) { - outcome.unknown_api_id = true; - outcome.status = unknown_api_status; - return outcome; - } - const int64_t api_id = header["api_id"].get(); - outcome.api_id = api_id; + } // namespace - const json body = parse_or_empty(body_json); - auto get_double = [&](const char* key, double fallback = 0.0) { - return body.contains(key) ? body[key].get() : fallback; - }; - auto get_int = [&](const char* key, int fallback = 0) { - return body.contains(key) ? body[key].get() : fallback; - }; - auto get_bool = [&](const char* key, bool fallback = true) { - return body.contains(key) ? body[key].get() : fallback; - }; + RpcOutcome dispatch_rpc(const std::string& header_json, + const std::string& body_json, + int current_mode, + int64_t unknown_api_status) { + RpcOutcome outcome; - switch (api_id) { - case booster::CHANGE_MODE: { - outcome.action.kind = RpcActionKind::MODE_CHANGE; - outcome.action.mode_change.mode = get_int("mode", booster::UNKNOWN); - break; - } - case booster::MOVE: { - outcome.action.kind = RpcActionKind::WALK; - outcome.action.walk.vx = get_double("vx"); - outcome.action.walk.vy = get_double("vy"); - outcome.action.walk.vyaw = get_double("vyaw"); - break; - } - case booster::ROTATE_HEAD: { - outcome.action.kind = RpcActionKind::HEAD; - outcome.action.head.pitch = get_double("pitch"); - outcome.action.head.yaw = get_double("yaw"); - break; - } - case booster::LIE_DOWN: { - outcome.action.kind = RpcActionKind::LIE_DOWN; - break; - } - case booster::GET_UP: { - outcome.action.kind = RpcActionKind::GET_UP; - // GetUpRequest's default member initialiser (target_mode = SOCCER) is what - // NUbots' plain GetUp() call expects — no body fields to read. - break; - } - case booster::GET_UP_WITH_MODE: { - outcome.action.kind = RpcActionKind::GET_UP; - outcome.action.get_up.target_mode = get_int("mode", k1sim::message::GetUpRequest{}.target_mode); - break; - } - case booster::VISUAL_KICK: { - outcome.action.kind = RpcActionKind::VISUAL_KICK; - outcome.action.visual_kick.start = get_bool("start", k1sim::message::VisualKickRequest{}.start); - outcome.action.visual_kick.version = get_int("version", k1sim::message::VisualKickRequest{}.version); - break; - } - case booster::GET_MODE: { - outcome.action.kind = RpcActionKind::NONE; - outcome.response_body = json{{"mode", current_mode}}.dump(); - break; - } - default: { + const json header = parse_or_empty(header_json); + if (!header.contains("api_id")) { outcome.unknown_api_id = true; outcome.status = unknown_api_status; - break; + return outcome; + } + const int64_t api_id = header["api_id"].get(); + outcome.api_id = api_id; + + const json body = parse_or_empty(body_json); + auto get_double = [&](const char* key, double fallback = 0.0) { + return body.contains(key) ? body[key].get() : fallback; + }; + auto get_int = [&](const char* key, int fallback = 0) { + return body.contains(key) ? body[key].get() : fallback; + }; + auto get_bool = [&](const char* key, bool fallback = true) { + return body.contains(key) ? body[key].get() : fallback; + }; + + switch (api_id) { + case booster::CHANGE_MODE: { + outcome.action.kind = RpcActionKind::MODE_CHANGE; + outcome.action.mode_change.mode = get_int("mode", booster::UNKNOWN); + break; + } + case booster::MOVE: { + outcome.action.kind = RpcActionKind::WALK; + outcome.action.walk.vx = get_double("vx"); + outcome.action.walk.vy = get_double("vy"); + outcome.action.walk.vyaw = get_double("vyaw"); + break; + } + case booster::ROTATE_HEAD: { + outcome.action.kind = RpcActionKind::HEAD; + outcome.action.head.pitch = get_double("pitch"); + outcome.action.head.yaw = get_double("yaw"); + break; + } + case booster::LIE_DOWN: { + outcome.action.kind = RpcActionKind::LIE_DOWN; + break; + } + case booster::GET_UP: { + outcome.action.kind = RpcActionKind::GET_UP; + // GetUpRequest's default member initialiser (target_mode = SOCCER) is what + // NUbots' plain GetUp() call expects — no body fields to read. + break; + } + case booster::GET_UP_WITH_MODE: { + outcome.action.kind = RpcActionKind::GET_UP; + outcome.action.get_up.target_mode = get_int("mode", k1sim::message::GetUpRequest{}.target_mode); + break; + } + case booster::VISUAL_KICK: { + outcome.action.kind = RpcActionKind::VISUAL_KICK; + outcome.action.visual_kick.start = get_bool("start", k1sim::message::VisualKickRequest{}.start); + outcome.action.visual_kick.version = get_int("version", k1sim::message::VisualKickRequest{}.version); + break; + } + case booster::GET_MODE: { + outcome.action.kind = RpcActionKind::NONE; + outcome.response_body = json{{"mode", current_mode}}.dump(); + break; + } + default: { + outcome.unknown_api_id = true; + outcome.status = unknown_api_status; + break; + } } - } - return outcome; -} + return outcome; + } } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/RpcDispatch.hpp b/mujoco/module/SdkBridge/src/RpcDispatch.hpp index 38e531d..6439a32 100644 --- a/mujoco/module/SdkBridge/src/RpcDispatch.hpp +++ b/mujoco/module/SdkBridge/src/RpcDispatch.hpp @@ -15,44 +15,44 @@ namespace k1sim::module::sdkbridge { -// Discriminates which (if any) NUClear message dispatch_rpc() wants emitted. Exactly -// one of the payload fields below is meaningful, selected by `kind`. -enum class RpcActionKind { - NONE, // GET_MODE (handled entirely via the reply body) and unknown api_ids - MODE_CHANGE, - WALK, - HEAD, - GET_UP, - LIE_DOWN, - VISUAL_KICK, -}; - -struct RpcAction { - RpcActionKind kind = RpcActionKind::NONE; - k1sim::message::ModeChangeRequest mode_change; - k1sim::message::WalkCommand walk; - k1sim::message::HeadCommand head; - k1sim::message::GetUpRequest get_up; - k1sim::message::LieDownRequest lie_down; - k1sim::message::VisualKickRequest visual_kick; -}; - -struct RpcOutcome { - RpcAction action; - int64_t status = 0; // goes in the reply header {"status": status} - std::string response_body = "{}"; // reply body; only GET_MODE returns non-trivial content - bool unknown_api_id = false; // true => caller should log a WARN - int64_t api_id = 0; // parsed api_id, for logging -}; - -// Parses `header_json` (RpcReqMsg_.header, expected `{"api_id": N, ...}`) and -// `body_json` (RpcReqMsg_.body, a per-call JSON object or empty string) and returns -// what to do. Never throws: malformed JSON or a missing/unrecognised api_id is -// reported via `unknown_api_id` with `status == unknown_api_status`. -RpcOutcome dispatch_rpc(const std::string& header_json, - const std::string& body_json, - int current_mode, - int64_t unknown_api_status); + // Discriminates which (if any) NUClear message dispatch_rpc() wants emitted. Exactly + // one of the payload fields below is meaningful, selected by `kind`. + enum class RpcActionKind { + NONE, // GET_MODE (handled entirely via the reply body) and unknown api_ids + MODE_CHANGE, + WALK, + HEAD, + GET_UP, + LIE_DOWN, + VISUAL_KICK, + }; + + struct RpcAction { + RpcActionKind kind = RpcActionKind::NONE; + k1sim::message::ModeChangeRequest mode_change; + k1sim::message::WalkCommand walk; + k1sim::message::HeadCommand head; + k1sim::message::GetUpRequest get_up; + k1sim::message::LieDownRequest lie_down; + k1sim::message::VisualKickRequest visual_kick; + }; + + struct RpcOutcome { + RpcAction action; + int64_t status = 0; // goes in the reply header {"status": status} + std::string response_body = "{}"; // reply body; only GET_MODE returns non-trivial content + bool unknown_api_id = false; // true => caller should log a WARN + int64_t api_id = 0; // parsed api_id, for logging + }; + + // Parses `header_json` (RpcReqMsg_.header, expected `{"api_id": N, ...}`) and + // `body_json` (RpcReqMsg_.body, a per-call JSON object or empty string) and returns + // what to do. Never throws: malformed JSON or a missing/unrecognised api_id is + // reported via `unknown_api_id` with `status == unknown_api_status`. + RpcOutcome dispatch_rpc(const std::string& header_json, + const std::string& body_json, + int current_mode, + int64_t unknown_api_status); } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/RpcServer.cpp b/mujoco/module/SdkBridge/src/RpcServer.cpp index a7ba891..a32a641 100644 --- a/mujoco/module/SdkBridge/src/RpcServer.cpp +++ b/mujoco/module/SdkBridge/src/RpcServer.cpp @@ -11,118 +11,123 @@ #include "RpcReqMsgPubSubTypes.h" #include "RpcRespMsg.h" #include "RpcRespMsgPubSubTypes.h" - #include "module/SdkBridge/src/RpcDispatch.hpp" #include "shared/k1/BoosterApi.hpp" #include "shared/message/Commands.hpp" namespace k1sim::module::sdkbridge { -using eprosima::fastdds::dds::DataReader; -using eprosima::fastdds::dds::SampleInfo; -using eprosima::fastrtps::types::ReturnCode_t; - -RpcServer::RpcServer(DdsParticipant& dds, NUClear::Reactor& reactor, int64_t unknown_api_status) - : reactor_(reactor), unknown_api_status_(unknown_api_status) { - - rpc_req_reader_ = dds.create_reader( - k1sim::booster::TOPIC_RPC_REQUEST, DdsParticipant::rpc_request_reader_qos(), this); - - rpc_resp_writer_ = dds.create_writer( - k1sim::booster::TOPIC_RPC_RESPONSE, DdsParticipant::state_writer_qos()); - - // rt/joint_ctrl (LowCmd_) — optional passthrough, only honoured by Locomotion in - // RobotMode::CUSTOM. Reuse the RPC reader's RELIABLE/KEEP_LAST QoS; this topic - // isn't latency-critical enough to warrant its own tuned profile. - joint_ctrl_reader_ = dds.create_reader( - k1sim::booster::TOPIC_JOINT_CTRL, DdsParticipant::rpc_request_reader_qos(5), this); -} - -void RpcServer::on_data_available(DataReader* reader) { - if (reader == rpc_req_reader_) { - handle_rpc_request(); - } - else if (reader == joint_ctrl_reader_) { - handle_joint_ctrl(); + using eprosima::fastdds::dds::DataReader; + using eprosima::fastdds::dds::SampleInfo; + using eprosima::fastrtps::types::ReturnCode_t; + + RpcServer::RpcServer(DdsParticipant& dds, NUClear::Reactor& reactor, int64_t unknown_api_status) + : reactor_(reactor), unknown_api_status_(unknown_api_status) { + + rpc_req_reader_ = + dds.create_reader(k1sim::booster::TOPIC_RPC_REQUEST, + DdsParticipant::rpc_request_reader_qos(), + this); + + rpc_resp_writer_ = + dds.create_writer(k1sim::booster::TOPIC_RPC_RESPONSE, + DdsParticipant::state_writer_qos()); + + // rt/joint_ctrl (LowCmd_) — optional passthrough, only honoured by Locomotion in + // RobotMode::CUSTOM. Reuse the RPC reader's RELIABLE/KEEP_LAST QoS; this topic + // isn't latency-critical enough to warrant its own tuned profile. + joint_ctrl_reader_ = dds.create_reader( + k1sim::booster::TOPIC_JOINT_CTRL, + DdsParticipant::rpc_request_reader_qos(5), + this); } -} - -void RpcServer::handle_rpc_request() { - booster_msgs::msg::dds_::RpcReqMsg_ req; - SampleInfo info; - while (rpc_req_reader_->take_next_sample(&req, &info) == ReturnCode_t::RETCODE_OK) { - if (!info.valid_data) { - continue; - } - - const RpcOutcome outcome = - dispatch_rpc(req.header(), req.body(), current_mode_.load(std::memory_order_relaxed), unknown_api_status_); - if (outcome.unknown_api_id) { - reactor_.log("SdkBridge: RPC unknown/unparseable api_id", - outcome.api_id, - "— replying status", - outcome.status); + void RpcServer::on_data_available(DataReader* reader) { + if (reader == rpc_req_reader_) { + handle_rpc_request(); } - - switch (outcome.action.kind) { - case RpcActionKind::MODE_CHANGE: - reactor_.emit(std::make_unique(outcome.action.mode_change)); - break; - case RpcActionKind::WALK: - reactor_.emit(std::make_unique(outcome.action.walk)); - break; - case RpcActionKind::HEAD: - reactor_.emit(std::make_unique(outcome.action.head)); - break; - case RpcActionKind::GET_UP: - reactor_.emit(std::make_unique(outcome.action.get_up)); - break; - case RpcActionKind::LIE_DOWN: - reactor_.emit(std::make_unique(outcome.action.lie_down)); - break; - case RpcActionKind::VISUAL_KICK: - reactor_.emit(std::make_unique(outcome.action.visual_kick)); - break; - case RpcActionKind::NONE: - default: - break; + else if (reader == joint_ctrl_reader_) { + handle_joint_ctrl(); } - - // Reply IMMEDIATELY, synchronously, in this DDS callback — B1LocoClient - // blocks up to 1000 ms per call (PROTOCOL.md §3). - booster_msgs::msg::dds_::RpcRespMsg_ resp; - resp.uuid(req.uuid()); - resp.header(nlohmann::json{{"status", outcome.status}}.dump()); - resp.body(outcome.response_body); - rpc_resp_writer_->write(&resp); } -} - -void RpcServer::handle_joint_ctrl() { - booster_interface::msg::dds_::LowCmd_ cmd; - SampleInfo info; - while (joint_ctrl_reader_->take_next_sample(&cmd, &info) == ReturnCode_t::RETCODE_OK) { - if (!info.valid_data) { - continue; + + void RpcServer::handle_rpc_request() { + booster_msgs::msg::dds_::RpcReqMsg_ req; + SampleInfo info; + while (rpc_req_reader_->take_next_sample(&req, &info) == ReturnCode_t::RETCODE_OK) { + if (!info.valid_data) { + continue; + } + + const RpcOutcome outcome = dispatch_rpc(req.header(), + req.body(), + current_mode_.load(std::memory_order_relaxed), + unknown_api_status_); + + if (outcome.unknown_api_id) { + reactor_.log("SdkBridge: RPC unknown/unparseable api_id", + outcome.api_id, + "— replying status", + outcome.status); + } + + switch (outcome.action.kind) { + case RpcActionKind::MODE_CHANGE: + reactor_.emit(std::make_unique(outcome.action.mode_change)); + break; + case RpcActionKind::WALK: + reactor_.emit(std::make_unique(outcome.action.walk)); + break; + case RpcActionKind::HEAD: + reactor_.emit(std::make_unique(outcome.action.head)); + break; + case RpcActionKind::GET_UP: + reactor_.emit(std::make_unique(outcome.action.get_up)); + break; + case RpcActionKind::LIE_DOWN: + reactor_.emit(std::make_unique(outcome.action.lie_down)); + break; + case RpcActionKind::VISUAL_KICK: + reactor_.emit(std::make_unique(outcome.action.visual_kick)); + break; + case RpcActionKind::NONE: + default: break; + } + + // Reply IMMEDIATELY, synchronously, in this DDS callback — B1LocoClient + // blocks up to 1000 ms per call (PROTOCOL.md §3). + booster_msgs::msg::dds_::RpcRespMsg_ resp; + resp.uuid(req.uuid()); + resp.header(nlohmann::json{{"status", outcome.status}}.dump()); + resp.body(outcome.response_body); + rpc_resp_writer_->write(&resp); } + } - auto msg = std::make_unique(); - msg->cmd_type = static_cast(cmd.cmd_type()); - msg->motors.reserve(cmd.motor_cmd().size()); - for (const auto& m : cmd.motor_cmd()) { - k1sim::message::MotorCmdData d; - d.mode = m.mode(); - d.q = m.q(); - d.dq = m.dq(); - d.tau = m.tau(); - d.kp = m.kp(); - d.kd = m.kd(); - d.weight = m.weight(); - msg->motors.push_back(d); + void RpcServer::handle_joint_ctrl() { + booster_interface::msg::dds_::LowCmd_ cmd; + SampleInfo info; + while (joint_ctrl_reader_->take_next_sample(&cmd, &info) == ReturnCode_t::RETCODE_OK) { + if (!info.valid_data) { + continue; + } + + auto msg = std::make_unique(); + msg->cmd_type = static_cast(cmd.cmd_type()); + msg->motors.reserve(cmd.motor_cmd().size()); + for (const auto& m : cmd.motor_cmd()) { + k1sim::message::MotorCmdData d; + d.mode = m.mode(); + d.q = m.q(); + d.dq = m.dq(); + d.tau = m.tau(); + d.kp = m.kp(); + d.kd = m.kd(); + d.weight = m.weight(); + msg->motors.push_back(d); + } + reactor_.emit(msg); } - reactor_.emit(msg); } -} } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/RpcServer.hpp b/mujoco/module/SdkBridge/src/RpcServer.hpp index ad40b01..fe24362 100644 --- a/mujoco/module/SdkBridge/src/RpcServer.hpp +++ b/mujoco/module/SdkBridge/src/RpcServer.hpp @@ -22,28 +22,30 @@ namespace k1sim::module::sdkbridge { -class RpcServer : public eprosima::fastdds::dds::DataReaderListener { -public: - RpcServer(DdsParticipant& dds, NUClear::Reactor& reactor, int64_t unknown_api_status); - - void on_data_available(eprosima::fastdds::dds::DataReader* reader) override; - - // SdkBridge's on> handler calls this every tick so - // GET_MODE can answer from an atomic without touching the physics thread. - void set_current_mode(int mode) { current_mode_.store(mode, std::memory_order_relaxed); } - -private: - void handle_rpc_request(); - void handle_joint_ctrl(); - - NUClear::Reactor& reactor_; - int64_t unknown_api_status_; - std::atomic current_mode_{0}; - - eprosima::fastdds::dds::DataReader* rpc_req_reader_ = nullptr; - eprosima::fastdds::dds::DataReader* joint_ctrl_reader_ = nullptr; - eprosima::fastdds::dds::DataWriter* rpc_resp_writer_ = nullptr; -}; + class RpcServer : public eprosima::fastdds::dds::DataReaderListener { + public: + RpcServer(DdsParticipant& dds, NUClear::Reactor& reactor, int64_t unknown_api_status); + + void on_data_available(eprosima::fastdds::dds::DataReader* reader) override; + + // SdkBridge's on> handler calls this every tick so + // GET_MODE can answer from an atomic without touching the physics thread. + void set_current_mode(int mode) { + current_mode_.store(mode, std::memory_order_relaxed); + } + + private: + void handle_rpc_request(); + void handle_joint_ctrl(); + + NUClear::Reactor& reactor_; + int64_t unknown_api_status_; + std::atomic current_mode_{0}; + + eprosima::fastdds::dds::DataReader* rpc_req_reader_ = nullptr; + eprosima::fastdds::dds::DataReader* joint_ctrl_reader_ = nullptr; + eprosima::fastdds::dds::DataWriter* rpc_resp_writer_ = nullptr; + }; } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/SdkBridge.cpp b/mujoco/module/SdkBridge/src/SdkBridge.cpp index 8ef7cc5..907d500 100644 --- a/mujoco/module/SdkBridge/src/SdkBridge.cpp +++ b/mujoco/module/SdkBridge/src/SdkBridge.cpp @@ -9,56 +9,53 @@ namespace k1sim::module { -namespace { -bool env_flag_set(const char* name) { - const char* value = std::getenv(name); - return value != nullptr && std::string(value) != "0" && std::string(value) != ""; -} -} // namespace - -SdkBridge::SdkBridge(std::unique_ptr environment) : Reactor(std::move(environment)) { - - on().then([this] { - auto cfg = config::load("dds.yaml"); - - const int domain = cfg["domain"].as(0); - const bool udp_only = cfg["udp_only"].as(false) || env_flag_set("K1_DDS_UDP_ONLY"); - const double battery_soc = cfg["battery_soc"].as(100.0); - const int64_t unknown_api_status = cfg["unknown_api_status"].as(0); - - dds_ = std::make_unique(domain, udp_only); - state_publisher_ = std::make_unique(*dds_, battery_soc); - rpc_server_ = std::make_unique(*dds_, *this, unknown_api_status); - - log("SdkBridge ready (DDS domain", - domain, - udp_only ? "UDP-only" : "UDP+SHM", - ")"); - }); - - // 50 Hz (matches SimCore's state_publish_divisor) — write low_state/odometer_state - // every tick, fall_down on change/keepalive, and cache the mode for GET_MODE. - on>().then([this](const message::SimStateUpdate& update) { - if (!rpc_server_ || !state_publisher_) { - return; // race with on — harmless, next tick will publish - } - rpc_server_->set_current_mode(update.mode); - state_publisher_->publish(update); - }); - - on>().then([this] { - if (state_publisher_) { - state_publisher_->publish_battery(); + namespace { + bool env_flag_set(const char* name) { + const char* value = std::getenv(name); + return value != nullptr && std::string(value) != "0" && std::string(value) != ""; } - }); - - on().then([this] { - log("SdkBridge shutting down"); - // Destroy in reverse-dependency order: readers/writers before the participant. - rpc_server_.reset(); - state_publisher_.reset(); - dds_.reset(); - }); -} + } // namespace + + SdkBridge::SdkBridge(std::unique_ptr environment) : Reactor(std::move(environment)) { + + on().then([this] { + auto cfg = config::load("dds.yaml"); + + const int domain = cfg["domain"].as(0); + const bool udp_only = cfg["udp_only"].as(false) || env_flag_set("K1_DDS_UDP_ONLY"); + const double battery_soc = cfg["battery_soc"].as(100.0); + const int64_t unknown_api_status = cfg["unknown_api_status"].as(0); + + dds_ = std::make_unique(domain, udp_only); + state_publisher_ = std::make_unique(*dds_, battery_soc); + rpc_server_ = std::make_unique(*dds_, *this, unknown_api_status); + + log("SdkBridge ready (DDS domain", domain, udp_only ? "UDP-only" : "UDP+SHM", ")"); + }); + + // 50 Hz (matches SimCore's state_publish_divisor) — write low_state/odometer_state + // every tick, fall_down on change/keepalive, and cache the mode for GET_MODE. + on>().then([this](const message::SimStateUpdate& update) { + if (!rpc_server_ || !state_publisher_) { + return; // race with on — harmless, next tick will publish + } + rpc_server_->set_current_mode(update.mode); + state_publisher_->publish(update); + }); + + on>().then([this] { + if (state_publisher_) { + state_publisher_->publish_battery(); + } + }); + + on().then([this] { + log("SdkBridge shutting down"); + // Destroy in reverse-dependency order: readers/writers before the participant. + rpc_server_.reset(); + state_publisher_.reset(); + dds_.reset(); + }); + } } // namespace k1sim::module diff --git a/mujoco/module/SdkBridge/src/SdkBridge.hpp b/mujoco/module/SdkBridge/src/SdkBridge.hpp index 884f510..5a80544 100644 --- a/mujoco/module/SdkBridge/src/SdkBridge.hpp +++ b/mujoco/module/SdkBridge/src/SdkBridge.hpp @@ -10,21 +10,21 @@ namespace k1sim::module { -// The Booster SDK compatibility surface: FastDDS publishers for -// rt/low_state, rt/odometer_state, rt/fall_down, rt/battery_state, -// rt/button_event and the LocoApi RPC server (rt/LocoApiTopicReq/Resp). -// See module/SdkBridge/PROTOCOL.md for the full wire contract this implements. -class SdkBridge : public NUClear::Reactor { -public: - explicit SdkBridge(std::unique_ptr environment); + // The Booster SDK compatibility surface: FastDDS publishers for + // rt/low_state, rt/odometer_state, rt/fall_down, rt/battery_state, + // rt/button_event and the LocoApi RPC server (rt/LocoApiTopicReq/Resp). + // See module/SdkBridge/PROTOCOL.md for the full wire contract this implements. + class SdkBridge : public NUClear::Reactor { + public: + explicit SdkBridge(std::unique_ptr environment); -private: - // Constructed in on (after config/dds.yaml is read) — see DdsParticipant - // for why a live participant can't be created before that. - std::unique_ptr dds_; - std::unique_ptr state_publisher_; - std::unique_ptr rpc_server_; -}; + private: + // Constructed in on (after config/dds.yaml is read) — see DdsParticipant + // for why a live participant can't be created before that. + std::unique_ptr dds_; + std::unique_ptr state_publisher_; + std::unique_ptr rpc_server_; + }; } // namespace k1sim::module diff --git a/mujoco/module/SdkBridge/src/StatePublisher.cpp b/mujoco/module/SdkBridge/src/StatePublisher.cpp index 0a45989..6a15934 100644 --- a/mujoco/module/SdkBridge/src/StatePublisher.cpp +++ b/mujoco/module/SdkBridge/src/StatePublisher.cpp @@ -15,136 +15,134 @@ #include "OdometerPubSubTypes.h" #include "Pose.h" #include "PosePubSubTypes.h" - #include "shared/k1/BoosterApi.hpp" #include "shared/k1/JointIndex.hpp" namespace k1sim::module::sdkbridge { -namespace { - -// BaseState::quat is {w, x, y, z} (see shared/message/SimMessages.hpp). Planar yaw -// extraction for the Odometer's theta field. -double yaw_from_quat(const std::array& q) { - const double w = q[0], x = q[1], y = q[2], z = q[3]; - return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); -} - -booster_interface::msg::dds_::MotorState_ to_motor_state(const k1sim::message::JointState& joint) { - booster_interface::msg::dds_::MotorState_ m; - m.mode(1); - m.q(static_cast(joint.q)); - m.dq(static_cast(joint.dq)); - m.ddq(static_cast(joint.ddq)); - m.tau_est(static_cast(joint.tau)); - m.temperature(40); - m.lost(0); - m.reserve({0, 0}); - return m; -} - -} // namespace - -StatePublisher::StatePublisher(DdsParticipant& dds, double battery_soc) : battery_soc_(battery_soc) { - using booster_interface::msg::dds_::BatteryState_PubSubType; - using booster_interface::msg::dds_::ButtonEventMsg_PubSubType; - using booster_interface::msg::dds_::FallDownState_PubSubType; - using booster_interface::msg::dds_::LowState_PubSubType; - using booster_interface::msg::dds_::Odometer_PubSubType; - using geometry_msgs::msg::dds_::Pose_PubSubType; - - low_state_writer_ = - dds.create_writer(k1sim::booster::TOPIC_LOW_STATE, DdsParticipant::state_writer_qos()); - odometer_writer_ = dds.create_writer(k1sim::booster::TOPIC_ODOMETER_STATE, - DdsParticipant::state_writer_qos()); - head_pose_writer_ = - dds.create_writer(k1sim::booster::TOPIC_HEAD_POSE, DdsParticipant::state_writer_qos()); - fall_down_writer_ = - dds.create_writer(k1sim::booster::TOPIC_FALL_DOWN, DdsParticipant::state_writer_qos()); - battery_writer_ = dds.create_writer(k1sim::booster::TOPIC_BATTERY_STATE, + namespace { + + // BaseState::quat is {w, x, y, z} (see shared/message/SimMessages.hpp). Planar yaw + // extraction for the Odometer's theta field. + double yaw_from_quat(const std::array& q) { + const double w = q[0], x = q[1], y = q[2], z = q[3]; + return std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + } + + booster_interface::msg::dds_::MotorState_ to_motor_state(const k1sim::message::JointState& joint) { + booster_interface::msg::dds_::MotorState_ m; + m.mode(1); + m.q(static_cast(joint.q)); + m.dq(static_cast(joint.dq)); + m.ddq(static_cast(joint.ddq)); + m.tau_est(static_cast(joint.tau)); + m.temperature(40); + m.lost(0); + m.reserve({0, 0}); + return m; + } + + } // namespace + + StatePublisher::StatePublisher(DdsParticipant& dds, double battery_soc) : battery_soc_(battery_soc) { + using booster_interface::msg::dds_::BatteryState_PubSubType; + using booster_interface::msg::dds_::ButtonEventMsg_PubSubType; + using booster_interface::msg::dds_::FallDownState_PubSubType; + using booster_interface::msg::dds_::LowState_PubSubType; + using booster_interface::msg::dds_::Odometer_PubSubType; + using geometry_msgs::msg::dds_::Pose_PubSubType; + + low_state_writer_ = + dds.create_writer(k1sim::booster::TOPIC_LOW_STATE, DdsParticipant::state_writer_qos()); + odometer_writer_ = dds.create_writer(k1sim::booster::TOPIC_ODOMETER_STATE, DdsParticipant::state_writer_qos()); - // Created so the topic/type exist on the wire (a real subscriber could match), - // but per the M4/M5 spec we never actually write to it — the sim has no buttons. - button_event_writer_ = dds.create_writer(k1sim::booster::TOPIC_BUTTON_EVENT, - DdsParticipant::state_writer_qos()); -} - -void StatePublisher::publish(const k1sim::message::SimStateUpdate& update) { - // --- rt/low_state --- - booster_interface::msg::dds_::LowState_ low_state; - low_state.imu_state().rpy({static_cast(update.imu.rpy[0]), static_cast(update.imu.rpy[1]), - static_cast(update.imu.rpy[2])}); - low_state.imu_state().gyro({static_cast(update.imu.gyro[0]), static_cast(update.imu.gyro[1]), - static_cast(update.imu.gyro[2])}); - low_state.imu_state().acc({static_cast(update.imu.acc[0]), static_cast(update.imu.acc[1]), - static_cast(update.imu.acc[2])}); - - std::vector motors; - motors.reserve(k1sim::JOINT_COUNT); - for (std::size_t i = 0; i < k1sim::JOINT_COUNT; ++i) { - motors.push_back(to_motor_state(update.joints[i])); - } - low_state.motor_state_serial(motors); - // motor_state_parallel mirrors the real firmware's layout: the 12 leg motors only - // (JointIndexK1 kLeftHipPitch..kCrankDownRight => vector indices 0..11), with the - // crank slots carrying the serial-equivalent ankle pitch/roll values (the sim has - // no true parallel actuation). Clients (e.g. NUbots HardwareIO) index this vector - // as joint_index - kLeftHipPitch. - std::vector legs(motors.begin() + JointIndexK1::LeftHipPitch, - motors.end()); - low_state.motor_state_parallel(legs); - low_state_writer_->write(&low_state); - - // --- rt/odometer_state --- - booster_interface::msg::dds_::Odometer_ odom; - odom.x(static_cast(update.base.x)); - odom.y(static_cast(update.base.y)); - odom.theta(static_cast(yaw_from_quat(update.base.quat))); - odometer_writer_->write(&odom); - - // --- rt/head_pose --- The head frame in the yaw-only base footprint frame, which - // K1Sensors composes with the odometry above to place the camera and torso in the world. - if (update.head.valid) { - geometry_msgs::msg::dds_::Pose_ pose; - pose.position().x(update.head.position[0]); - pose.position().y(update.head.position[1]); - pose.position().z(update.head.position[2]); - pose.orientation().w(update.head.quat[0]); - pose.orientation().x(update.head.quat[1]); - pose.orientation().y(update.head.quat[2]); - pose.orientation().z(update.head.quat[3]); - head_pose_writer_->write(&pose); + head_pose_writer_ = + dds.create_writer(k1sim::booster::TOPIC_HEAD_POSE, DdsParticipant::state_writer_qos()); + fall_down_writer_ = dds.create_writer(k1sim::booster::TOPIC_FALL_DOWN, + DdsParticipant::state_writer_qos()); + battery_writer_ = dds.create_writer(k1sim::booster::TOPIC_BATTERY_STATE, + DdsParticipant::state_writer_qos()); + // Created so the topic/type exist on the wire (a real subscriber could match), + // but per the M4/M5 spec we never actually write to it — the sim has no buttons. + button_event_writer_ = dds.create_writer(k1sim::booster::TOPIC_BUTTON_EVENT, + DdsParticipant::state_writer_qos()); } - // --- rt/fall_down --- publish on change, or every >=1s as a keepalive. - const bool changed = !have_last_fall_state_ || last_fall_state_ != update.fall_state; - const bool due_keepalive = (update.sim_time - last_fall_publish_time_) >= 1.0; - if (changed || due_keepalive) { - if (changed) { - std::fprintf(stderr, - "StatePublisher: fall_state -> %d (t=%.2f)\n", - update.fall_state, - update.sim_time); + void StatePublisher::publish(const k1sim::message::SimStateUpdate& update) { + // --- rt/low_state --- + booster_interface::msg::dds_::LowState_ low_state; + low_state.imu_state().rpy({static_cast(update.imu.rpy[0]), + static_cast(update.imu.rpy[1]), + static_cast(update.imu.rpy[2])}); + low_state.imu_state().gyro({static_cast(update.imu.gyro[0]), + static_cast(update.imu.gyro[1]), + static_cast(update.imu.gyro[2])}); + low_state.imu_state().acc({static_cast(update.imu.acc[0]), + static_cast(update.imu.acc[1]), + static_cast(update.imu.acc[2])}); + + std::vector motors; + motors.reserve(k1sim::JOINT_COUNT); + for (std::size_t i = 0; i < k1sim::JOINT_COUNT; ++i) { + motors.push_back(to_motor_state(update.joints[i])); } - booster_interface::msg::dds_::FallDownState_ fall; - fall.fall_down_state( - static_cast(update.fall_state)); - fall.is_recovery_available(true); - fall_down_writer_->write(&fall); - - have_last_fall_state_ = true; - last_fall_state_ = update.fall_state; - last_fall_publish_time_ = update.sim_time; + low_state.motor_state_serial(motors); + // motor_state_parallel mirrors the real firmware's layout: the 12 leg motors only + // (JointIndexK1 kLeftHipPitch..kCrankDownRight => vector indices 0..11), with the + // crank slots carrying the serial-equivalent ankle pitch/roll values (the sim has + // no true parallel actuation). Clients (e.g. NUbots HardwareIO) index this vector + // as joint_index - kLeftHipPitch. + std::vector legs(motors.begin() + JointIndexK1::LeftHipPitch, + motors.end()); + low_state.motor_state_parallel(legs); + low_state_writer_->write(&low_state); + + // --- rt/odometer_state --- + booster_interface::msg::dds_::Odometer_ odom; + odom.x(static_cast(update.base.x)); + odom.y(static_cast(update.base.y)); + odom.theta(static_cast(yaw_from_quat(update.base.quat))); + odometer_writer_->write(&odom); + + // --- rt/head_pose --- The head frame in the yaw-only base footprint frame, which + // K1Sensors composes with the odometry above to place the camera and torso in the world. + if (update.head.valid) { + geometry_msgs::msg::dds_::Pose_ pose; + pose.position().x(update.head.position[0]); + pose.position().y(update.head.position[1]); + pose.position().z(update.head.position[2]); + pose.orientation().w(update.head.quat[0]); + pose.orientation().x(update.head.quat[1]); + pose.orientation().y(update.head.quat[2]); + pose.orientation().z(update.head.quat[3]); + head_pose_writer_->write(&pose); + } + + // --- rt/fall_down --- publish on change, or every >=1s as a keepalive. + const bool changed = !have_last_fall_state_ || last_fall_state_ != update.fall_state; + const bool due_keepalive = (update.sim_time - last_fall_publish_time_) >= 1.0; + if (changed || due_keepalive) { + if (changed) { + std::fprintf(stderr, "StatePublisher: fall_state -> %d (t=%.2f)\n", update.fall_state, update.sim_time); + } + booster_interface::msg::dds_::FallDownState_ fall; + fall.fall_down_state(static_cast(update.fall_state)); + fall.is_recovery_available(true); + fall_down_writer_->write(&fall); + + have_last_fall_state_ = true; + last_fall_state_ = update.fall_state; + last_fall_publish_time_ = update.sim_time; + } + } + + void StatePublisher::publish_battery() { + booster_interface::msg::dds_::BatteryState_ battery; + battery.voltage(0.0f); + battery.current(0.0f); + battery.soc(static_cast(battery_soc_)); + battery.average_voltage(0.0f); + battery_writer_->write(&battery); } -} - -void StatePublisher::publish_battery() { - booster_interface::msg::dds_::BatteryState_ battery; - battery.voltage(0.0f); - battery.current(0.0f); - battery.soc(static_cast(battery_soc_)); - battery.average_voltage(0.0f); - battery_writer_->write(&battery); -} } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/src/StatePublisher.hpp b/mujoco/module/SdkBridge/src/StatePublisher.hpp index ac3253a..19d3b92 100644 --- a/mujoco/module/SdkBridge/src/StatePublisher.hpp +++ b/mujoco/module/SdkBridge/src/StatePublisher.hpp @@ -11,33 +11,33 @@ namespace k1sim::module::sdkbridge { -class StatePublisher { -public: - // battery_soc comes from config/dds.yaml (constant, published at 1 Hz). - StatePublisher(DdsParticipant& dds, double battery_soc); - - // Called at the LowState cadence (50 Hz, on>): writes - // rt/low_state, rt/odometer_state and rt/head_pose every call, and rt/fall_down - // whenever fall_state changes or >=1s has elapsed since the last publish (keepalive). - void publish(const k1sim::message::SimStateUpdate& update); - - // Called on>: writes the constant-SOC rt/battery_state. - void publish_battery(); - -private: - eprosima::fastdds::dds::DataWriter* low_state_writer_ = nullptr; - eprosima::fastdds::dds::DataWriter* odometer_writer_ = nullptr; - eprosima::fastdds::dds::DataWriter* head_pose_writer_ = nullptr; - eprosima::fastdds::dds::DataWriter* fall_down_writer_ = nullptr; - eprosima::fastdds::dds::DataWriter* battery_writer_ = nullptr; - eprosima::fastdds::dds::DataWriter* button_event_writer_ = nullptr; // created, never published - - double battery_soc_; - - bool have_last_fall_state_ = false; - int last_fall_state_ = -1; - double last_fall_publish_time_ = -1e9; -}; + class StatePublisher { + public: + // battery_soc comes from config/dds.yaml (constant, published at 1 Hz). + StatePublisher(DdsParticipant& dds, double battery_soc); + + // Called at the LowState cadence (50 Hz, on>): writes + // rt/low_state, rt/odometer_state and rt/head_pose every call, and rt/fall_down + // whenever fall_state changes or >=1s has elapsed since the last publish (keepalive). + void publish(const k1sim::message::SimStateUpdate& update); + + // Called on>: writes the constant-SOC rt/battery_state. + void publish_battery(); + + private: + eprosima::fastdds::dds::DataWriter* low_state_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* odometer_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* head_pose_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* fall_down_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* battery_writer_ = nullptr; + eprosima::fastdds::dds::DataWriter* button_event_writer_ = nullptr; // created, never published + + double battery_soc_; + + bool have_last_fall_state_ = false; + int last_fall_state_ = -1; + double last_fall_publish_time_ = -1e9; + }; } // namespace k1sim::module::sdkbridge diff --git a/mujoco/module/SdkBridge/test_support/CMakeLists.txt b/mujoco/module/SdkBridge/test_support/CMakeLists.txt index 84e10de..70ebec7 100644 --- a/mujoco/module/SdkBridge/test_support/CMakeLists.txt +++ b/mujoco/module/SdkBridge/test_support/CMakeLists.txt @@ -1,8 +1,5 @@ -# sdkbridge_synthetic_sim: test-only binary used by test/contract/test_sdk_roundtrip.py -# to exercise the real module::SdkBridge without depending on module::Simulation/ -# Locomotion/Viewer being functional yet. See SyntheticState.hpp for rationale. +# sdkbridge_synthetic_sim: test-only binary used by test/contract/test_sdk_roundtrip.py to exercise the real +# module::SdkBridge without depending on module::Simulation/ Locomotion/Viewer being functional yet. See +# SyntheticState.hpp for rationale. add_executable(sdkbridge_synthetic_sim main.cpp SyntheticState.cpp) -target_link_libraries( - sdkbridge_synthetic_sim - PRIVATE k1sim_shared k1sim_module_consolelog k1sim_module_sdkbridge -) +target_link_libraries(sdkbridge_synthetic_sim PRIVATE k1sim_shared k1sim_module_consolelog k1sim_module_sdkbridge) diff --git a/mujoco/module/SdkBridge/test_support/SyntheticState.cpp b/mujoco/module/SdkBridge/test_support/SyntheticState.cpp index cb158b5..90d6699 100644 --- a/mujoco/module/SdkBridge/test_support/SyntheticState.cpp +++ b/mujoco/module/SdkBridge/test_support/SyntheticState.cpp @@ -9,99 +9,99 @@ namespace k1sim::module::sdkbridge::test_support { -namespace { -constexpr double kDt = 1.0 / 50.0; -} - -SyntheticState::SyntheticState(std::unique_ptr environment) : Reactor(std::move(environment)) { - - on().then([this] { - log("SyntheticState ready (test-only SimStateUpdate source, 50 Hz)"); - }); - - on>().then([this](const k1sim::message::ModeChangeRequest& msg) { - std::scoped_lock lock(mutex_); - mode_ = msg.mode; - }); - - on>().then([this](const k1sim::message::WalkCommand& msg) { - std::scoped_lock lock(mutex_); - vx_ = msg.vx; - vy_ = msg.vy; - vyaw_ = msg.vyaw; - }); - - on>().then([this](const k1sim::message::HeadCommand& msg) { - std::scoped_lock lock(mutex_); - head_pitch_ = msg.pitch; - head_yaw_ = msg.yaw; - }); - - on>().then([this](const k1sim::message::GetUpRequest& msg) { - std::scoped_lock lock(mutex_); - fall_state_ = k1sim::booster::IS_READY; - mode_ = msg.target_mode; - }); - - on>().then([this] { - std::scoped_lock lock(mutex_); - fall_state_ = k1sim::booster::HAS_FALLEN; - }); - - on>().then( - [this](const k1sim::message::VisualKickRequest& msg) { (void) msg; }); - - on>().then([this] { - auto update = std::make_unique(); - - std::scoped_lock lock(mutex_); - sim_time_ += kDt; - step_count_ += 1; - - // Planar odometry integration (body-frame vx/vy rotated by current yaw). - const double cos_yaw = std::cos(yaw_); - const double sin_yaw = std::sin(yaw_); - x_ += (vx_ * cos_yaw - vy_ * sin_yaw) * kDt; - y_ += (vx_ * sin_yaw + vy_ * cos_yaw) * kDt; - yaw_ += vyaw_ * kDt; - - update->sim_time = sim_time_; - update->step_count = step_count_; - - // Plausible standing IMU: level attitude, near-zero gyro except the - // commanded yaw rate, gravity on the accelerometer. - update->imu.rpy = {0.0, 0.0, yaw_}; - update->imu.gyro = {0.0, 0.0, vyaw_}; - update->imu.acc = {0.0, 0.0, 9.81}; - - for (std::size_t i = 0; i < k1sim::JOINT_COUNT; ++i) { - update->joints[i] = {}; - } - update->joints[k1sim::HeadYaw].q = head_yaw_; - update->joints[k1sim::HeadPitch].q = head_pitch_; - - update->base.x = x_; - update->base.y = y_; - update->base.z = 0.53; - const double half = yaw_ * 0.5; - update->base.quat = {std::cos(half), 0.0, 0.0, std::sin(half)}; - - // Standing head frame in the footprint frame: 0.33 m above the base (the Head_pitch - // joint 0.248 m above the Trunk plus the 0.08 m head frame offset, see - // shared/sim/HeadPose.hpp), turned by the commanded head yaw then pitch. - const double cy = std::cos(head_yaw_ * 0.5), sy = std::sin(head_yaw_ * 0.5); - const double cp = std::cos(head_pitch_ * 0.5), sp = std::sin(head_pitch_ * 0.5); - update->head.valid = true; - update->head.position = {0.0, 0.0, update->base.z + 0.33}; - update->head.quat = {cy * cp, -sy * sp, cy * sp, sy * cp}; - - update->mode = mode_; - update->fall_state = fall_state_; - update->getting_up = false; - update->measured_rtf = 1.0; - - emit(update); - }); -} + namespace { + constexpr double kDt = 1.0 / 50.0; + } + + SyntheticState::SyntheticState(std::unique_ptr environment) + : Reactor(std::move(environment)) { + + on().then( + [this] { log("SyntheticState ready (test-only SimStateUpdate source, 50 Hz)"); }); + + on>().then([this](const k1sim::message::ModeChangeRequest& msg) { + std::scoped_lock lock(mutex_); + mode_ = msg.mode; + }); + + on>().then([this](const k1sim::message::WalkCommand& msg) { + std::scoped_lock lock(mutex_); + vx_ = msg.vx; + vy_ = msg.vy; + vyaw_ = msg.vyaw; + }); + + on>().then([this](const k1sim::message::HeadCommand& msg) { + std::scoped_lock lock(mutex_); + head_pitch_ = msg.pitch; + head_yaw_ = msg.yaw; + }); + + on>().then([this](const k1sim::message::GetUpRequest& msg) { + std::scoped_lock lock(mutex_); + fall_state_ = k1sim::booster::IS_READY; + mode_ = msg.target_mode; + }); + + on>().then([this] { + std::scoped_lock lock(mutex_); + fall_state_ = k1sim::booster::HAS_FALLEN; + }); + + on>().then( + [this](const k1sim::message::VisualKickRequest& msg) { (void) msg; }); + + on>().then([this] { + auto update = std::make_unique(); + + std::scoped_lock lock(mutex_); + sim_time_ += kDt; + step_count_ += 1; + + // Planar odometry integration (body-frame vx/vy rotated by current yaw). + const double cos_yaw = std::cos(yaw_); + const double sin_yaw = std::sin(yaw_); + x_ += (vx_ * cos_yaw - vy_ * sin_yaw) * kDt; + y_ += (vx_ * sin_yaw + vy_ * cos_yaw) * kDt; + yaw_ += vyaw_ * kDt; + + update->sim_time = sim_time_; + update->step_count = step_count_; + + // Plausible standing IMU: level attitude, near-zero gyro except the + // commanded yaw rate, gravity on the accelerometer. + update->imu.rpy = {0.0, 0.0, yaw_}; + update->imu.gyro = {0.0, 0.0, vyaw_}; + update->imu.acc = {0.0, 0.0, 9.81}; + + for (std::size_t i = 0; i < k1sim::JOINT_COUNT; ++i) { + update->joints[i] = {}; + } + update->joints[k1sim::HeadYaw].q = head_yaw_; + update->joints[k1sim::HeadPitch].q = head_pitch_; + + update->base.x = x_; + update->base.y = y_; + update->base.z = 0.53; + const double half = yaw_ * 0.5; + update->base.quat = {std::cos(half), 0.0, 0.0, std::sin(half)}; + + // Standing head frame in the footprint frame: 0.33 m above the base (the Head_pitch + // joint 0.248 m above the Trunk plus the 0.08 m head frame offset, see + // shared/sim/HeadPose.hpp), turned by the commanded head yaw then pitch. + const double cy = std::cos(head_yaw_ * 0.5), sy = std::sin(head_yaw_ * 0.5); + const double cp = std::cos(head_pitch_ * 0.5), sp = std::sin(head_pitch_ * 0.5); + update->head.valid = true; + update->head.position = {0.0, 0.0, update->base.z + 0.33}; + update->head.quat = {cy * cp, -sy * sp, cy * sp, sy * cp}; + + update->mode = mode_; + update->fall_state = fall_state_; + update->getting_up = false; + update->measured_rtf = 1.0; + + emit(update); + }); + } } // namespace k1sim::module::sdkbridge::test_support diff --git a/mujoco/module/SdkBridge/test_support/SyntheticState.hpp b/mujoco/module/SdkBridge/test_support/SyntheticState.hpp index f50a767..ed05678 100644 --- a/mujoco/module/SdkBridge/test_support/SyntheticState.hpp +++ b/mujoco/module/SdkBridge/test_support/SyntheticState.hpp @@ -18,23 +18,23 @@ namespace k1sim::module::sdkbridge::test_support { -class SyntheticState : public NUClear::Reactor { -public: - explicit SyntheticState(std::unique_ptr environment); + class SyntheticState : public NUClear::Reactor { + public: + explicit SyntheticState(std::unique_ptr environment); -private: - std::mutex mutex_; - double sim_time_ = 0.0; - uint64_t step_count_ = 0; + private: + std::mutex mutex_; + double sim_time_ = 0.0; + uint64_t step_count_ = 0; - int mode_ = 0; // booster::RobotMode::DAMPING - int fall_state_ = 0; // booster::FallState::IS_READY + int mode_ = 0; // booster::RobotMode::DAMPING + int fall_state_ = 0; // booster::FallState::IS_READY - double x_ = 0.0, y_ = 0.0, yaw_ = 0.0; // planar odometry, integrated from WalkCommand - double vx_ = 0.0, vy_ = 0.0, vyaw_ = 0.0; + double x_ = 0.0, y_ = 0.0, yaw_ = 0.0; // planar odometry, integrated from WalkCommand + double vx_ = 0.0, vy_ = 0.0, vyaw_ = 0.0; - double head_pitch_ = 0.0, head_yaw_ = 0.0; -}; + double head_pitch_ = 0.0, head_yaw_ = 0.0; + }; } // namespace k1sim::module::sdkbridge::test_support diff --git a/mujoco/module/SdkBridge/test_support/main.cpp b/mujoco/module/SdkBridge/test_support/main.cpp index 377aa8a..70f2b56 100644 --- a/mujoco/module/SdkBridge/test_support/main.cpp +++ b/mujoco/module/SdkBridge/test_support/main.cpp @@ -10,11 +10,11 @@ #include "module/SdkBridge/test_support/SyntheticState.hpp" namespace { -void handle_signal(int /*signum*/) { - if (NUClear::PowerPlant::powerplant != nullptr) { - NUClear::PowerPlant::powerplant->shutdown(); + void handle_signal(int /*signum*/) { + if (NUClear::PowerPlant::powerplant != nullptr) { + NUClear::PowerPlant::powerplant->shutdown(); + } } -} } // namespace int main() { diff --git a/mujoco/module/Simulation/CMakeLists.txt b/mujoco/module/Simulation/CMakeLists.txt index 1ba960b..e68cb0e 100644 --- a/mujoco/module/Simulation/CMakeLists.txt +++ b/mujoco/module/Simulation/CMakeLists.txt @@ -1,2 +1,2 @@ -add_library(k1sim_module_simulation STATIC src/Simulation.cpp src/SimCore.cpp) +add_library(k1sim_module_simulation STATIC src/SimCore.cpp src/Simulation.cpp) target_link_libraries(k1sim_module_simulation PUBLIC k1sim_shared) diff --git a/mujoco/module/Simulation/src/SimCore.cpp b/mujoco/module/Simulation/src/SimCore.cpp index 68f986f..1570f35 100644 --- a/mujoco/module/Simulation/src/SimCore.cpp +++ b/mujoco/module/Simulation/src/SimCore.cpp @@ -11,622 +11,623 @@ namespace k1sim { -namespace { - -// ZYX (yaw-pitch-roll) Euler extraction from a wxyz quaternion — the conventional -// roll/pitch/yaw decomposition used by the Booster wire format's imu_state.rpy. -void quat_to_rpy(const std::array& q, std::array& rpy) { - const double w = q[0]; - const double x = q[1]; - const double y = q[2]; - const double z = q[3]; - - const double sinr_cosp = 2.0 * (w * x + y * z); - const double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); - rpy[0] = std::atan2(sinr_cosp, cosr_cosp); - - double sinp = 2.0 * (w * y - z * x); - sinp = std::clamp(sinp, -1.0, 1.0); - rpy[1] = std::asin(sinp); - - const double siny_cosp = 2.0 * (w * z + x * y); - const double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); - rpy[2] = std::atan2(siny_cosp, cosy_cosp); -} - -double to_seconds(const timespec& t) { - return static_cast(t.tv_sec) + static_cast(t.tv_nsec) * 1e-9; -} - -timespec add_seconds(timespec t, double seconds) { - const double total_nsec = static_cast(t.tv_nsec) + seconds * 1e9; - auto sec_adjust = static_cast(std::floor(total_nsec / 1e9)); - t.tv_sec += sec_adjust; - t.tv_nsec = static_cast(total_nsec - static_cast(sec_adjust) * 1e9); - return t; -} - -// Name prefix for the k-th extra robot's joints/actuators/bodies (k starts at 1). -std::string extra_prefix(int k) { - char buf[16]; - std::snprintf(buf, sizeof(buf), "sub%02d_", k); - return buf; -} - -// Spawn slot for the k-th extra robot (k starts at 1): a 5x4 grid on the field, -// rows at y = +-1.2 / +-2.4 so nothing lands on the y=0 line the main robot and -// ball spawn on. z is the "ready" standing base height. The scene keyframes -// zero-pad the extra free joints (global coords — zeros mean "at the origin, -// inside the main robot"), so these slots are written into qpos explicitly -// after every keyframe reset rather than relying on the padding. -std::array extra_spawn(int k) { - const int col = (k - 1) % 5; - const int row = (k - 1) / 5; - const double x = -3.0 + 1.5 * col; - const double y = (row % 2 == 0 ? 1.0 : -1.0) * (1.2 + 1.2 * (row / 2)); - return {x, y, 0.555}; -} - -// Build a scene with (robots - 1) extra K1 copies attached via mjSpec. Each copy's -// element names get a "subNN_" prefix, so the main robot's unprefixed names (and -// every existing name-based lookup) stay valid. Extra copies' keyframes are dropped; -// the parent scene's keyframes zero-pad the new free joints, landing each copy at -// its attachment frame. -mjModel* load_multi_robot_model(const std::string& scene_path, int robots, char* error, int error_sz) { - mjSpec* scene = mj_parseXML(scene_path.c_str(), nullptr, error, error_sz); - if (scene == nullptr) { - throw std::runtime_error("mj_parseXML failed for '" + scene_path + "': " + error); - } - - const std::string robot_xml = - scene_path.substr(0, scene_path.find_last_of('/') + 1) + "K1_22dof.xml"; + namespace { + + // ZYX (yaw-pitch-roll) Euler extraction from a wxyz quaternion — the conventional + // roll/pitch/yaw decomposition used by the Booster wire format's imu_state.rpy. + void quat_to_rpy(const std::array& q, std::array& rpy) { + const double w = q[0]; + const double x = q[1]; + const double y = q[2]; + const double z = q[3]; + + const double sinr_cosp = 2.0 * (w * x + y * z); + const double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); + rpy[0] = std::atan2(sinr_cosp, cosr_cosp); + + double sinp = 2.0 * (w * y - z * x); + sinp = std::clamp(sinp, -1.0, 1.0); + rpy[1] = std::asin(sinp); + + const double siny_cosp = 2.0 * (w * z + x * y); + const double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + rpy[2] = std::atan2(siny_cosp, cosy_cosp); + } - mjsBody* world = mjs_findBody(scene, "world"); - for (int k = 1; k < robots; ++k) { - mjSpec* robot = mj_parseXML(robot_xml.c_str(), nullptr, error, error_sz); - if (robot == nullptr) { - mj_deleteSpec(scene); - throw std::runtime_error("mj_parseXML failed for '" + robot_xml + "': " + error); - } - // The copy inherits the scene keyframes' zero-padding; its own (robot-sized) - // keyframes would collide with the scene's on attach, so drop them. - for (mjsElement* key = mjs_firstElement(robot, mjOBJ_KEY); key != nullptr; - key = mjs_firstElement(robot, mjOBJ_KEY)) { - mjs_delete(robot, key); - } - - mjsFrame* frame = mjs_addFrame(world, nullptr); - const auto pos = extra_spawn(k); - frame->pos[0] = pos[0]; - frame->pos[1] = pos[1]; - frame->pos[2] = pos[2]; - mjsBody* trunk = mjs_findBody(robot, "Trunk"); - mjsElement* attached = mjs_attach(frame->element, trunk->element, extra_prefix(k).c_str(), ""); - if (attached == nullptr) { - const std::string what = std::string("mjs_attach failed for robot copy ") + std::to_string(k) - + ": " + mjs_getError(scene); - mj_deleteSpec(robot); - mj_deleteSpec(scene); - throw std::runtime_error(what); + double to_seconds(const timespec& t) { + return static_cast(t.tv_sec) + static_cast(t.tv_nsec) * 1e-9; } - mj_deleteSpec(robot); - } - mjModel* m = mj_compile(scene, nullptr); - if (m == nullptr) { - const std::string what = std::string("mj_compile failed for multi-robot scene: ") + mjs_getError(scene); - mj_deleteSpec(scene); - throw std::runtime_error(what); - } - mj_deleteSpec(scene); - return m; -} + timespec add_seconds(timespec t, double seconds) { + const double total_nsec = static_cast(t.tv_nsec) + seconds * 1e9; + auto sec_adjust = static_cast(std::floor(total_nsec / 1e9)); + t.tv_sec += sec_adjust; + t.tv_nsec = static_cast(total_nsec - static_cast(sec_adjust) * 1e9); + return t; + } -} // namespace + // Name prefix for the k-th extra robot's joints/actuators/bodies (k starts at 1). + std::string extra_prefix(int k) { + char buf[16]; + std::snprintf(buf, sizeof(buf), "sub%02d_", k); + return buf; + } -SimCore::SimCore(Config config, StateCallback on_state) : config_(std::move(config)), on_state_(std::move(on_state)) { - pd_.kp = config_.kp; - pd_.kd = config_.kd; -} + // Spawn slot for the k-th extra robot (k starts at 1): a 5x4 grid on the field, + // rows at y = +-1.2 / +-2.4 so nothing lands on the y=0 line the main robot and + // ball spawn on. z is the "ready" standing base height. The scene keyframes + // zero-pad the extra free joints (global coords — zeros mean "at the origin, + // inside the main robot"), so these slots are written into qpos explicitly + // after every keyframe reset rather than relying on the padding. + std::array extra_spawn(int k) { + const int col = (k - 1) % 5; + const int row = (k - 1) / 5; + const double x = -3.0 + 1.5 * col; + const double y = (row % 2 == 0 ? 1.0 : -1.0) * (1.2 + 1.2 * (row / 2)); + return {x, y, 0.555}; + } -SimCore::~SimCore() { - stop(); - unload(); -} + // Build a scene with (robots - 1) extra K1 copies attached via mjSpec. Each copy's + // element names get a "subNN_" prefix, so the main robot's unprefixed names (and + // every existing name-based lookup) stay valid. Extra copies' keyframes are dropped; + // the parent scene's keyframes zero-pad the new free joints, landing each copy at + // its attachment frame. + mjModel* load_multi_robot_model(const std::string& scene_path, int robots, char* error, int error_sz) { + mjSpec* scene = mj_parseXML(scene_path.c_str(), nullptr, error, error_sz); + if (scene == nullptr) { + throw std::runtime_error("mj_parseXML failed for '" + scene_path + "': " + error); + } -void SimCore::load_model() { - const std::string resolved = config::resolve_path(config_.model_path).string(); + const std::string robot_xml = scene_path.substr(0, scene_path.find_last_of('/') + 1) + "K1_22dof.xml"; + + mjsBody* world = mjs_findBody(scene, "world"); + for (int k = 1; k < robots; ++k) { + mjSpec* robot = mj_parseXML(robot_xml.c_str(), nullptr, error, error_sz); + if (robot == nullptr) { + mj_deleteSpec(scene); + throw std::runtime_error("mj_parseXML failed for '" + robot_xml + "': " + error); + } + // The copy inherits the scene keyframes' zero-padding; its own (robot-sized) + // keyframes would collide with the scene's on attach, so drop them. + for (mjsElement* key = mjs_firstElement(robot, mjOBJ_KEY); key != nullptr; + key = mjs_firstElement(robot, mjOBJ_KEY)) { + mjs_delete(robot, key); + } + + mjsFrame* frame = mjs_addFrame(world, nullptr); + const auto pos = extra_spawn(k); + frame->pos[0] = pos[0]; + frame->pos[1] = pos[1]; + frame->pos[2] = pos[2]; + mjsBody* trunk = mjs_findBody(robot, "Trunk"); + mjsElement* attached = mjs_attach(frame->element, trunk->element, extra_prefix(k).c_str(), ""); + if (attached == nullptr) { + const std::string what = std::string("mjs_attach failed for robot copy ") + std::to_string(k) + ": " + + mjs_getError(scene); + mj_deleteSpec(robot); + mj_deleteSpec(scene); + throw std::runtime_error(what); + } + mj_deleteSpec(robot); + } - char error[1024] = {0}; - if (config_.robots <= 1) { - m_ = mj_loadXML(resolved.c_str(), nullptr, error, sizeof(error)); - if (m_ == nullptr) { - throw std::runtime_error("mj_loadXML failed for '" + resolved + "': " + error); + mjModel* m = mj_compile(scene, nullptr); + if (m == nullptr) { + const std::string what = std::string("mj_compile failed for multi-robot scene: ") + mjs_getError(scene); + mj_deleteSpec(scene); + throw std::runtime_error(what); + } + mj_deleteSpec(scene); + return m; } + + } // namespace + + SimCore::SimCore(Config config, StateCallback on_state) + : config_(std::move(config)), on_state_(std::move(on_state)) { + pd_.kp = config_.kp; + pd_.kd = config_.kd; } - else { - m_ = load_multi_robot_model(resolved, config_.robots, error, sizeof(error)); + + SimCore::~SimCore() { + stop(); + unload(); } - // Throws if any joint/actuator is missing or there is no free root joint. - map_ = ModelMap::build(m_); + void SimCore::load_model() { + const std::string resolved = config::resolve_path(config_.model_path).string(); + + char error[1024] = {0}; + if (config_.robots <= 1) { + m_ = mj_loadXML(resolved.c_str(), nullptr, error, sizeof(error)); + if (m_ == nullptr) { + throw std::runtime_error("mj_loadXML failed for '" + resolved + "': " + error); + } + } + else { + m_ = load_multi_robot_model(resolved, config_.robots, error, sizeof(error)); + } + + // Throws if any joint/actuator is missing or there is no free root joint. + map_ = ModelMap::build(m_); - apply_surface_override(); + apply_surface_override(); - left_foot_body_id_ = mj_name2id(m_, mjOBJ_BODY, "left_foot_link"); - right_foot_body_id_ = mj_name2id(m_, mjOBJ_BODY, "right_foot_link"); + left_foot_body_id_ = mj_name2id(m_, mjOBJ_BODY, "left_foot_link"); + right_foot_body_id_ = mj_name2id(m_, mjOBJ_BODY, "right_foot_link"); - head_body_id_ = mj_name2id(m_, mjOBJ_BODY, "Head_2"); - if (head_body_id_ < 0) { - std::fprintf(stderr, "SimCore: model has no Head_2 body; rt/head_pose will not be published\n"); - } - if (!config_.foot_log_path.empty()) { - if (left_foot_body_id_ < 0 || right_foot_body_id_ < 0) { - std::fprintf(stderr, "SimCore: foot log requested but the model has no left/right_foot_link\n"); + head_body_id_ = mj_name2id(m_, mjOBJ_BODY, "Head_2"); + if (head_body_id_ < 0) { + std::fprintf(stderr, "SimCore: model has no Head_2 body; rt/head_pose will not be published\n"); } - else { - foot_log_ = std::fopen(config_.foot_log_path.c_str(), "w"); - if (foot_log_ == nullptr) { - std::fprintf(stderr, "SimCore: could not open foot log '%s'\n", config_.foot_log_path.c_str()); + if (!config_.foot_log_path.empty()) { + if (left_foot_body_id_ < 0 || right_foot_body_id_ < 0) { + std::fprintf(stderr, "SimCore: foot log requested but the model has no left/right_foot_link\n"); } else { - std::fprintf(foot_log_, - "t,l_fz,l_cop_x,l_cop_y,l_pitch,l_roll," - "r_fz,r_cop_x,r_cop_y,r_pitch,r_roll,base_pitch,base_z\n"); - std::fprintf(stderr, "SimCore: foot contact log -> %s\n", config_.foot_log_path.c_str()); + foot_log_ = std::fopen(config_.foot_log_path.c_str(), "w"); + if (foot_log_ == nullptr) { + std::fprintf(stderr, "SimCore: could not open foot log '%s'\n", config_.foot_log_path.c_str()); + } + else { + std::fprintf(foot_log_, + "t,l_fz,l_cop_x,l_cop_y,l_pitch,l_roll," + "r_fz,r_cop_x,r_cop_y,r_pitch,r_roll,base_pitch,base_z\n"); + std::fprintf(stderr, "SimCore: foot contact log -> %s\n", config_.foot_log_path.c_str()); + } } } - } - // Index maps for the extra robot copies so the physics loop can PD-hold them - // upright. Only the three per-joint index arrays are needed. - extra_maps_.clear(); - for (int k = 1; k < config_.robots; ++k) { - const std::string prefix = extra_prefix(k); - ModelMap em{}; - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - const std::string name = prefix + JOINT_NAMES[i]; - const int jnt = mj_name2id(m_, mjOBJ_JOINT, name.c_str()); - const int act = mj_name2id(m_, mjOBJ_ACTUATOR, name.c_str()); - if (jnt < 0 || act < 0) { - throw std::runtime_error("multi-robot scene is missing joint/actuator '" + name + "'"); + // Index maps for the extra robot copies so the physics loop can PD-hold them + // upright. Only the three per-joint index arrays are needed. + extra_maps_.clear(); + for (int k = 1; k < config_.robots; ++k) { + const std::string prefix = extra_prefix(k); + ModelMap em{}; + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + const std::string name = prefix + JOINT_NAMES[i]; + const int jnt = mj_name2id(m_, mjOBJ_JOINT, name.c_str()); + const int act = mj_name2id(m_, mjOBJ_ACTUATOR, name.c_str()); + if (jnt < 0 || act < 0) { + throw std::runtime_error("multi-robot scene is missing joint/actuator '" + name + "'"); + } + em.qpos_adr[i] = m_->jnt_qposadr[jnt]; + em.dof_adr[i] = m_->jnt_dofadr[jnt]; + em.act_id[i] = act; } - em.qpos_adr[i] = m_->jnt_qposadr[jnt]; - em.dof_adr[i] = m_->jnt_dofadr[jnt]; - em.act_id[i] = act; - } - const std::string root = prefix + "root"; - const int root_jnt = mj_name2id(m_, mjOBJ_JOINT, root.c_str()); - if (root_jnt < 0) { - throw std::runtime_error("multi-robot scene is missing free joint '" + root + "'"); - } - em.root_qpos_adr = m_->jnt_qposadr[root_jnt]; - em.root_dof_adr = m_->jnt_dofadr[root_jnt]; - extra_maps_.push_back(em); - } - - d_ = mj_makeData(m_); - if (d_ == nullptr) { - mj_deleteModel(m_); - m_ = nullptr; - throw std::runtime_error("mj_makeData failed for '" + resolved + "'"); - } - - // Spawn keyframe (configurable, e.g. lying_front for get-up testing); the PD - // fallback target always tracks the "ready" pose regardless of where we spawn. - int spawn_key = mj_name2id(m_, mjOBJ_KEY, config_.initial_keyframe.c_str()); - if (spawn_key < 0 && config_.initial_keyframe != "ready") { - std::fprintf(stderr, - "SimCore: model has no keyframe '%s'; falling back to 'ready'\n", - config_.initial_keyframe.c_str()); - spawn_key = mj_name2id(m_, mjOBJ_KEY, "ready"); - } - reset_key_ = spawn_key; - if (spawn_key >= 0) { - mj_resetDataKeyframe(m_, d_, spawn_key); - } - - const int ready_key = mj_name2id(m_, mjOBJ_KEY, "ready"); - if (ready_key >= 0) { - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - ready_target_[i] = m_->key_qpos[ready_key * m_->nq + map_.qpos_adr[i]]; + const std::string root = prefix + "root"; + const int root_jnt = mj_name2id(m_, mjOBJ_JOINT, root.c_str()); + if (root_jnt < 0) { + throw std::runtime_error("multi-robot scene is missing free joint '" + root + "'"); + } + em.root_qpos_adr = m_->jnt_qposadr[root_jnt]; + em.root_dof_adr = m_->jnt_dofadr[root_jnt]; + extra_maps_.push_back(em); } - } - else { - ready_target_ = config_.ready_pose_fallback; - } - - place_extras(); - - // Populate derived quantities (xquat, sensordata, ...) for the reset pose before the - // physics thread's first mj_step; harmless if nothing reads them this early. - mj_forward(m_, d_); -} - -// Make the floor's declared contact parameters the ones the feet actually see. -// -// MuJoCo derives a contact's parameters from the two geoms: if their priorities are equal -// it takes the element-wise MAX of the friction vectors. The K1 foot box carries no -// explicit friction, so it gets the MuJoCo default of 1.0, and max(1.0, floor) means the -// floor's number has never mattered -- a "0.8 grass" scene has been simulating mu = 1.0. -// Raising the floor's priority makes it authoritative, which is what the mujoco_playground -// training scene does (its floor geom is declared priority="1"). -// -// Explicit elements (the scene defines one for ball-vs-floor) are unaffected by -// priority, so tuned ball dynamics survive this. -void SimCore::apply_surface_override() { - if (!config_.surface.enabled) { - return; - } - const int floor = mj_name2id(m_, mjOBJ_GEOM, "floor"); - if (floor < 0) { - std::fprintf(stderr, "SimCore: surface override requested but the scene has no geom 'floor'\n"); - return; - } - - const double min_timeconst = 2.0 * m_->opt.timestep; - double timeconst = config_.surface.solref_timeconst; - if (timeconst < min_timeconst) { - std::fprintf(stderr, - "SimCore: surface.solref_timeconst %g is below 2*timestep (%g); clamping\n", - timeconst, - min_timeconst); - timeconst = min_timeconst; - } - m_->geom_priority[floor] = 1; - m_->geom_friction[3 * floor] = config_.surface.friction; - m_->geom_solref[2 * floor] = timeconst; - m_->geom_solref[2 * floor + 1] = config_.surface.solref_dampratio; - - std::fprintf(stderr, - "SimCore: floor contact overridden — mu = %g, solref = [%g, %g], priority = 1\n", - config_.surface.friction, - timeconst, - config_.surface.solref_dampratio); -} - -// One CSV row per published state: per foot the total contact normal force, the centre of -// pressure expressed in that foot's own frame, and the sole's pitch/roll. -// -// The centre of pressure is what makes tiptoe measurable rather than a description of a -// video. The sole box spans x in [-0.064, +0.116] of the foot frame, so a flat-footed -// stance sits near cop_x = 0.026 (the box centre) and a foot rolled onto its toe pushes -// cop_x towards +0.116 -- the front edge of the support polygon, where the available -// friction is spent on a shrinking contact patch. Caller holds mutex_. -void SimCore::log_foot_state() { - if (foot_log_ == nullptr) { - return; - } - - struct FootAcc { - double fz = 0.0; // summed normal force - double cop_x = 0.0; // force-weighted, foot frame - double cop_y = 0.0; - double pitch = 0.0; // sole tilt, world - double roll = 0.0; - }; - std::array feet{}; - const std::array body_ids{left_foot_body_id_, right_foot_body_id_}; - - for (int f = 0; f < 2; ++f) { - if (body_ids[f] < 0) { - continue; - } - // The foot's own +z axis expressed in world; its x/y components are the sole tilt. - const mjtNum* R = d_->xmat + 9 * body_ids[f]; - feet[f].pitch = std::atan2(R[0 * 3 + 2], R[2 * 3 + 2]); - feet[f].roll = std::atan2(R[1 * 3 + 2], R[2 * 3 + 2]); - } + d_ = mj_makeData(m_); + if (d_ == nullptr) { + mj_deleteModel(m_); + m_ = nullptr; + throw std::runtime_error("mj_makeData failed for '" + resolved + "'"); + } - for (int c = 0; c < d_->ncon; ++c) { - const mjContact& con = d_->contact[c]; - const int body1 = m_->geom_bodyid[con.geom1]; - const int body2 = m_->geom_bodyid[con.geom2]; - int f = -1; - if (body1 == left_foot_body_id_ || body2 == left_foot_body_id_) { - f = 0; + // Spawn keyframe (configurable, e.g. lying_front for get-up testing); the PD + // fallback target always tracks the "ready" pose regardless of where we spawn. + int spawn_key = mj_name2id(m_, mjOBJ_KEY, config_.initial_keyframe.c_str()); + if (spawn_key < 0 && config_.initial_keyframe != "ready") { + std::fprintf(stderr, + "SimCore: model has no keyframe '%s'; falling back to 'ready'\n", + config_.initial_keyframe.c_str()); + spawn_key = mj_name2id(m_, mjOBJ_KEY, "ready"); } - else if (body1 == right_foot_body_id_ || body2 == right_foot_body_id_) { - f = 1; + reset_key_ = spawn_key; + if (spawn_key >= 0) { + mj_resetDataKeyframe(m_, d_, spawn_key); } - if (f < 0) { - continue; + + const int ready_key = mj_name2id(m_, mjOBJ_KEY, "ready"); + if (ready_key >= 0) { + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + ready_target_[i] = m_->key_qpos[ready_key * m_->nq + map_.qpos_adr[i]]; + } + } + else { + ready_target_ = config_.ready_pose_fallback; } - // force[0] is the normal component in the contact frame. - mjtNum force[6] = {0}; - mj_contactForce(m_, d_, c, force); - const double fn = force[0]; - if (fn <= 0.0) { - continue; + place_extras(); + + // Populate derived quantities (xquat, sensordata, ...) for the reset pose before the + // physics thread's first mj_step; harmless if nothing reads them this early. + mj_forward(m_, d_); + } + + // Make the floor's declared contact parameters the ones the feet actually see. + // + // MuJoCo derives a contact's parameters from the two geoms: if their priorities are equal + // it takes the element-wise MAX of the friction vectors. The K1 foot box carries no + // explicit friction, so it gets the MuJoCo default of 1.0, and max(1.0, floor) means the + // floor's number has never mattered -- a "0.8 grass" scene has been simulating mu = 1.0. + // Raising the floor's priority makes it authoritative, which is what the mujoco_playground + // training scene does (its floor geom is declared priority="1"). + // + // Explicit elements (the scene defines one for ball-vs-floor) are unaffected by + // priority, so tuned ball dynamics survive this. + void SimCore::apply_surface_override() { + if (!config_.surface.enabled) { + return; + } + const int floor = mj_name2id(m_, mjOBJ_GEOM, "floor"); + if (floor < 0) { + std::fprintf(stderr, "SimCore: surface override requested but the scene has no geom 'floor'\n"); + return; } - // Contact point into the foot's frame: local = R^T * (pos - body_pos). - const mjtNum* R = d_->xmat + 9 * body_ids[f]; - const mjtNum* org = d_->xpos + 3 * body_ids[f]; - const mjtNum rel[3] = {con.pos[0] - org[0], con.pos[1] - org[1], con.pos[2] - org[2]}; - mjtNum local[3]; - mju_mulMatTVec3(local, R, rel); + const double min_timeconst = 2.0 * m_->opt.timestep; + double timeconst = config_.surface.solref_timeconst; + if (timeconst < min_timeconst) { + std::fprintf(stderr, + "SimCore: surface.solref_timeconst %g is below 2*timestep (%g); clamping\n", + timeconst, + min_timeconst); + timeconst = min_timeconst; + } - feet[f].fz += fn; - feet[f].cop_x += fn * local[0]; - feet[f].cop_y += fn * local[1]; - } + m_->geom_priority[floor] = 1; + m_->geom_friction[3 * floor] = config_.surface.friction; + m_->geom_solref[2 * floor] = timeconst; + m_->geom_solref[2 * floor + 1] = config_.surface.solref_dampratio; - for (auto& foot : feet) { - if (foot.fz > 0.0) { - foot.cop_x /= foot.fz; - foot.cop_y /= foot.fz; + std::fprintf(stderr, + "SimCore: floor contact overridden — mu = %g, solref = [%g, %g], priority = 1\n", + config_.surface.friction, + timeconst, + config_.surface.solref_dampratio); + } + + // One CSV row per published state: per foot the total contact normal force, the centre of + // pressure expressed in that foot's own frame, and the sole's pitch/roll. + // + // The centre of pressure is what makes tiptoe measurable rather than a description of a + // video. The sole box spans x in [-0.064, +0.116] of the foot frame, so a flat-footed + // stance sits near cop_x = 0.026 (the box centre) and a foot rolled onto its toe pushes + // cop_x towards +0.116 -- the front edge of the support polygon, where the available + // friction is spent on a shrinking contact patch. Caller holds mutex_. + void SimCore::log_foot_state() { + if (foot_log_ == nullptr) { + return; } - } - std::array quat{1, 0, 0, 0}; - if (map_.root_body_id >= 0) { - for (int k = 0; k < 4; ++k) { - quat[k] = d_->xquat[4 * map_.root_body_id + k]; - } - } - std::array rpy{}; - quat_to_rpy(quat, rpy); - - std::fprintf(foot_log_, - "%.4f,%.3f,%.5f,%.5f,%.5f,%.5f,%.3f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\n", - d_->time, - feet[0].fz, - feet[0].cop_x, - feet[0].cop_y, - feet[0].pitch, - feet[0].roll, - feet[1].fz, - feet[1].cop_x, - feet[1].cop_y, - feet[1].pitch, - feet[1].roll, - rpy[1], - d_->qpos[map_.root_qpos_adr + 2]); -} - -// Put every extra robot copy at its spawn slot in the ready pose with zero velocity. -// Keyframe resets zero-pad the extras' free joints (= world origin, inside the main -// robot), so this must run after every keyframe reset. Caller holds mutex_ (or the -// physics thread is not running yet). -void SimCore::place_extras() { - for (std::size_t j = 0; j < extra_maps_.size(); ++j) { - const ModelMap& em = extra_maps_[j]; - const auto pos = extra_spawn(static_cast(j) + 1); - d_->qpos[em.root_qpos_adr + 0] = pos[0]; - d_->qpos[em.root_qpos_adr + 1] = pos[1]; - d_->qpos[em.root_qpos_adr + 2] = pos[2]; - d_->qpos[em.root_qpos_adr + 3] = 1.0; // identity quat (w,x,y,z) - d_->qpos[em.root_qpos_adr + 4] = 0.0; - d_->qpos[em.root_qpos_adr + 5] = 0.0; - d_->qpos[em.root_qpos_adr + 6] = 0.0; - for (int v = 0; v < 6; ++v) { - d_->qvel[em.root_dof_adr + v] = 0.0; - } - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - d_->qpos[em.qpos_adr[i]] = ready_target_[i]; - d_->qvel[em.dof_adr[i]] = 0.0; + struct FootAcc { + double fz = 0.0; // summed normal force + double cop_x = 0.0; // force-weighted, foot frame + double cop_y = 0.0; + double pitch = 0.0; // sole tilt, world + double roll = 0.0; + }; + std::array feet{}; + const std::array body_ids{left_foot_body_id_, right_foot_body_id_}; + + for (int f = 0; f < 2; ++f) { + if (body_ids[f] < 0) { + continue; + } + // The foot's own +z axis expressed in world; its x/y components are the sole tilt. + const mjtNum* R = d_->xmat + 9 * body_ids[f]; + feet[f].pitch = std::atan2(R[0 * 3 + 2], R[2 * 3 + 2]); + feet[f].roll = std::atan2(R[1 * 3 + 2], R[2 * 3 + 2]); } - } -} -void SimCore::reset() { - std::lock_guard lock(mutex_); - if (reset_key_ >= 0) { - mj_resetDataKeyframe(m_, d_, reset_key_); - } - else { - mj_resetData(m_, d_); - } - place_extras(); - // Repopulate derived quantities so snapshots/viewer frames between now and the next - // mj_step see the reset pose, not stale kinematics. - mj_forward(m_, d_); -} - -void SimCore::start() { - bool expected = false; - if (!running_.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { - return; // already running - } - thread_ = std::thread(&SimCore::physics_loop, this); -} + for (int c = 0; c < d_->ncon; ++c) { + const mjContact& con = d_->contact[c]; + const int body1 = m_->geom_bodyid[con.geom1]; + const int body2 = m_->geom_bodyid[con.geom2]; + int f = -1; + if (body1 == left_foot_body_id_ || body2 == left_foot_body_id_) { + f = 0; + } + else if (body1 == right_foot_body_id_ || body2 == right_foot_body_id_) { + f = 1; + } + if (f < 0) { + continue; + } -void SimCore::stop() { - running_.store(false, std::memory_order_release); - if (thread_.joinable()) { - thread_.join(); - } -} + // force[0] is the normal component in the contact frame. + mjtNum force[6] = {0}; + mj_contactForce(m_, d_, c, force); + const double fn = force[0]; + if (fn <= 0.0) { + continue; + } -void SimCore::unload() { - if (foot_log_ != nullptr) { - std::fclose(foot_log_); - foot_log_ = nullptr; - } - if (d_ != nullptr) { - mj_deleteData(d_); - d_ = nullptr; - } - if (m_ != nullptr) { - mj_deleteModel(m_); - m_ = nullptr; - } -} - -std::unique_ptr SimCore::make_snapshot(uint64_t steps) const { - auto s = std::make_unique(); - s->sim_time = d_->time; - s->step_count = steps; - - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - auto& j = s->joints[i]; - j.q = d_->qpos[map_.qpos_adr[i]]; - j.dq = d_->qvel[map_.dof_adr[i]]; - j.ddq = d_->qacc[map_.dof_adr[i]]; - j.tau = d_->actuator_force[map_.act_id[i]]; - } + // Contact point into the foot's frame: local = R^T * (pos - body_pos). + const mjtNum* R = d_->xmat + 9 * body_ids[f]; + const mjtNum* org = d_->xpos + 3 * body_ids[f]; + const mjtNum rel[3] = {con.pos[0] - org[0], con.pos[1] - org[1], con.pos[2] - org[2]}; + mjtNum local[3]; + mju_mulMatTVec3(local, R, rel); - // IMU orientation + rpy. - std::array quat{1, 0, 0, 0}; - if (map_.sens_quat >= 0) { - for (int k = 0; k < 4; ++k) { - quat[k] = d_->sensordata[map_.sens_quat + k]; + feet[f].fz += fn; + feet[f].cop_x += fn * local[0]; + feet[f].cop_y += fn * local[1]; } - } - else if (map_.root_body_id >= 0) { - for (int k = 0; k < 4; ++k) { - quat[k] = d_->xquat[4 * map_.root_body_id + k]; + + for (auto& foot : feet) { + if (foot.fz > 0.0) { + foot.cop_x /= foot.fz; + foot.cop_y /= foot.fz; + } } - } - s->imu.quat = quat; - quat_to_rpy(quat, s->imu.rpy); - if (map_.sens_gyro >= 0) { - for (int k = 0; k < 3; ++k) { - s->imu.gyro[k] = d_->sensordata[map_.sens_gyro + k]; + std::array quat{1, 0, 0, 0}; + if (map_.root_body_id >= 0) { + for (int k = 0; k < 4; ++k) { + quat[k] = d_->xquat[4 * map_.root_body_id + k]; + } + } + std::array rpy{}; + quat_to_rpy(quat, rpy); + + std::fprintf(foot_log_, + "%.4f,%.3f,%.5f,%.5f,%.5f,%.5f,%.3f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\n", + d_->time, + feet[0].fz, + feet[0].cop_x, + feet[0].cop_y, + feet[0].pitch, + feet[0].roll, + feet[1].fz, + feet[1].cop_x, + feet[1].cop_y, + feet[1].pitch, + feet[1].roll, + rpy[1], + d_->qpos[map_.root_qpos_adr + 2]); + } + + // Put every extra robot copy at its spawn slot in the ready pose with zero velocity. + // Keyframe resets zero-pad the extras' free joints (= world origin, inside the main + // robot), so this must run after every keyframe reset. Caller holds mutex_ (or the + // physics thread is not running yet). + void SimCore::place_extras() { + for (std::size_t j = 0; j < extra_maps_.size(); ++j) { + const ModelMap& em = extra_maps_[j]; + const auto pos = extra_spawn(static_cast(j) + 1); + d_->qpos[em.root_qpos_adr + 0] = pos[0]; + d_->qpos[em.root_qpos_adr + 1] = pos[1]; + d_->qpos[em.root_qpos_adr + 2] = pos[2]; + d_->qpos[em.root_qpos_adr + 3] = 1.0; // identity quat (w,x,y,z) + d_->qpos[em.root_qpos_adr + 4] = 0.0; + d_->qpos[em.root_qpos_adr + 5] = 0.0; + d_->qpos[em.root_qpos_adr + 6] = 0.0; + for (int v = 0; v < 6; ++v) { + d_->qvel[em.root_dof_adr + v] = 0.0; + } + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + d_->qpos[em.qpos_adr[i]] = ready_target_[i]; + d_->qvel[em.dof_adr[i]] = 0.0; + } } - } - else if (map_.root_body_id >= 0) { - // Fallback: cvel's angular part is world-axis-aligned; rotate into the body's local - // frame with the body rotation matrix (R^T * world = mju_mulMatTVec3). - const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; - const mjtNum world[3] = {d_->cvel[6 * map_.root_body_id + 0], - d_->cvel[6 * map_.root_body_id + 1], - d_->cvel[6 * map_.root_body_id + 2]}; - mjtNum local[3]; - mju_mulMatTVec3(local, rmat, world); - s->imu.gyro = {local[0], local[1], local[2]}; } - if (map_.sens_acc >= 0) { - for (int k = 0; k < 3; ++k) { - s->imu.acc[k] = d_->sensordata[map_.sens_acc + k]; + void SimCore::reset() { + std::lock_guard lock(mutex_); + if (reset_key_ >= 0) { + mj_resetDataKeyframe(m_, d_, reset_key_); } - } - else if (map_.root_body_id >= 0) { - // Fallback approximation: the reading a stationary accelerometer would show under - // gravity alone (R^T * (0,0,+g)); ignores true linear acceleration (no cacc bookkeeping - // without the real sensor). Good enough for a defensive path that isn't exercised by - // the vendored model, which always carries the real sensor. - const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; - const mjtNum g = -m_->opt.gravity[2]; - const mjtNum world[3] = {0, 0, g}; - mjtNum local[3]; - mju_mulMatTVec3(local, rmat, world); - s->imu.acc = {local[0], local[1], local[2]}; + else { + mj_resetData(m_, d_); + } + place_extras(); + // Repopulate derived quantities so snapshots/viewer frames between now and the next + // mj_step see the reset pose, not stale kinematics. + mj_forward(m_, d_); } - // Base pose/velocity (free root joint). qpos: [x y z qw qx qy qz]; qvel: [vx vy vz wx wy wz] - // with the linear part in world frame and the angular part in the body's local frame. - const int qadr = map_.root_qpos_adr; - const int vadr = map_.root_dof_adr; - s->base.x = d_->qpos[qadr + 0]; - s->base.y = d_->qpos[qadr + 1]; - s->base.z = d_->qpos[qadr + 2]; - s->base.quat = {d_->qpos[qadr + 3], d_->qpos[qadr + 4], d_->qpos[qadr + 5], d_->qpos[qadr + 6]}; - s->base.lin_vel = {d_->qvel[vadr + 0], d_->qvel[vadr + 1], d_->qvel[vadr + 2]}; - { - const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; - const mjtNum local[3] = {d_->qvel[vadr + 3], d_->qvel[vadr + 4], d_->qvel[vadr + 5]}; - mjtNum world[3]; - mju_mulMatVec3(world, rmat, local); - s->base.ang_vel = {world[0], world[1], world[2]}; + void SimCore::start() { + bool expected = false; + if (!running_.compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { + return; // already running + } + thread_ = std::thread(&SimCore::physics_loop, this); } - if (head_body_id_ >= 0) { - const FootprintPose Hrh = head_in_footprint(d_->xpos + 3 * head_body_id_, - d_->xquat + 4 * head_body_id_, - d_->qpos + qadr, - d_->qpos + qadr + 3); - s->head.valid = true; - s->head.position = Hrh.position; - s->head.quat = Hrh.quat; + void SimCore::stop() { + running_.store(false, std::memory_order_release); + if (thread_.joinable()) { + thread_.join(); + } } - StepController* ctrl = controller_.load(std::memory_order_acquire); - if (ctrl != nullptr) { - s->mode = ctrl->mode(); - s->fall_state = ctrl->fall_state(); - s->getting_up = ctrl->getting_up(); - } - else { - s->mode = booster::RobotMode::PREPARE; - s->fall_state = booster::FallState::IS_READY; - s->getting_up = false; + void SimCore::unload() { + if (foot_log_ != nullptr) { + std::fclose(foot_log_); + foot_log_ = nullptr; + } + if (d_ != nullptr) { + mj_deleteData(d_); + d_ = nullptr; + } + if (m_ != nullptr) { + mj_deleteModel(m_); + m_ = nullptr; + } } - s->measured_rtf = measured_rtf_.load(std::memory_order_relaxed); - return s; -} - -void SimCore::physics_loop() { - const double dt = m_->opt.timestep; - const bool free_run = config_.rtf <= 0.0; - const double period = free_run ? 0.0 : dt / config_.rtf; - const auto publish_every = static_cast(config_.state_publish_divisor > 0 ? config_.state_publish_divisor : 0); - - timespec deadline{}; - clock_gettime(CLOCK_MONOTONIC, &deadline); - - double window_wall_start = to_seconds(deadline); - uint64_t window_step_start = 0; - uint64_t steps = 0; + std::unique_ptr SimCore::make_snapshot(uint64_t steps) const { + auto s = std::make_unique(); + s->sim_time = d_->time; + s->step_count = steps; - while (running_.load(std::memory_order_acquire)) { - std::unique_ptr snapshot; - { - std::lock_guard lock(mutex_); + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + auto& j = s->joints[i]; + j.q = d_->qpos[map_.qpos_adr[i]]; + j.dq = d_->qvel[map_.dof_adr[i]]; + j.ddq = d_->qacc[map_.dof_adr[i]]; + j.tau = d_->actuator_force[map_.act_id[i]]; + } - StepController* ctrl = controller_.load(std::memory_order_acquire); - if (ctrl != nullptr) { - ctrl->step(m_, d_); + // IMU orientation + rpy. + std::array quat{1, 0, 0, 0}; + if (map_.sens_quat >= 0) { + for (int k = 0; k < 4; ++k) { + quat[k] = d_->sensordata[map_.sens_quat + k]; } - else { - pd_.apply(m_, d_, map_, ready_target_); + } + else if (map_.root_body_id >= 0) { + for (int k = 0; k < 4; ++k) { + quat[k] = d_->xquat[4 * map_.root_body_id + k]; } - // Extra --robots copies have no controller; hold them at the ready pose. - for (const auto& em : extra_maps_) { - pd_.apply(m_, d_, em, ready_target_); + } + s->imu.quat = quat; + quat_to_rpy(quat, s->imu.rpy); + + if (map_.sens_gyro >= 0) { + for (int k = 0; k < 3; ++k) { + s->imu.gyro[k] = d_->sensordata[map_.sens_gyro + k]; } - mj_step(m_, d_); - ++steps; - step_count_.store(steps, std::memory_order_relaxed); + } + else if (map_.root_body_id >= 0) { + // Fallback: cvel's angular part is world-axis-aligned; rotate into the body's local + // frame with the body rotation matrix (R^T * world = mju_mulMatTVec3). + const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; + const mjtNum world[3] = {d_->cvel[6 * map_.root_body_id + 0], + d_->cvel[6 * map_.root_body_id + 1], + d_->cvel[6 * map_.root_body_id + 2]}; + mjtNum local[3]; + mju_mulMatTVec3(local, rmat, world); + s->imu.gyro = {local[0], local[1], local[2]}; + } - if (publish_every > 0 && steps % publish_every == 0) { - snapshot = make_snapshot(steps); - log_foot_state(); + if (map_.sens_acc >= 0) { + for (int k = 0; k < 3; ++k) { + s->imu.acc[k] = d_->sensordata[map_.sens_acc + k]; } - } // release the mutex before emitting/pacing + } + else if (map_.root_body_id >= 0) { + // Fallback approximation: the reading a stationary accelerometer would show under + // gravity alone (R^T * (0,0,+g)); ignores true linear acceleration (no cacc bookkeeping + // without the real sensor). Good enough for a defensive path that isn't exercised by + // the vendored model, which always carries the real sensor. + const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; + const mjtNum g = -m_->opt.gravity[2]; + const mjtNum world[3] = {0, 0, g}; + mjtNum local[3]; + mju_mulMatTVec3(local, rmat, world); + s->imu.acc = {local[0], local[1], local[2]}; + } - if (snapshot && on_state_) { - on_state_(std::move(snapshot)); + // Base pose/velocity (free root joint). qpos: [x y z qw qx qy qz]; qvel: [vx vy vz wx wy wz] + // with the linear part in world frame and the angular part in the body's local frame. + const int qadr = map_.root_qpos_adr; + const int vadr = map_.root_dof_adr; + s->base.x = d_->qpos[qadr + 0]; + s->base.y = d_->qpos[qadr + 1]; + s->base.z = d_->qpos[qadr + 2]; + s->base.quat = {d_->qpos[qadr + 3], d_->qpos[qadr + 4], d_->qpos[qadr + 5], d_->qpos[qadr + 6]}; + s->base.lin_vel = {d_->qvel[vadr + 0], d_->qvel[vadr + 1], d_->qvel[vadr + 2]}; + { + const mjtNum* rmat = d_->xmat + 9 * map_.root_body_id; + const mjtNum local[3] = {d_->qvel[vadr + 3], d_->qvel[vadr + 4], d_->qvel[vadr + 5]}; + mjtNum world[3]; + mju_mulMatVec3(world, rmat, local); + s->base.ang_vel = {world[0], world[1], world[2]}; } - if (!free_run) { - deadline = add_seconds(deadline, period); - timespec now_ts{}; - clock_gettime(CLOCK_MONOTONIC, &now_ts); - const double behind = to_seconds(now_ts) - to_seconds(deadline); - if (behind > config_.resync_threshold) { - deadline = now_ts; - dropped_deadlines_.fetch_add(1, std::memory_order_relaxed); - } - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &deadline, nullptr); + if (head_body_id_ >= 0) { + const FootprintPose Hrh = head_in_footprint(d_->xpos + 3 * head_body_id_, + d_->xquat + 4 * head_body_id_, + d_->qpos + qadr, + d_->qpos + qadr + 3); + s->head.valid = true; + s->head.position = Hrh.position; + s->head.quat = Hrh.quat; } - timespec wall_now{}; - clock_gettime(CLOCK_MONOTONIC, &wall_now); - const double wall_elapsed = to_seconds(wall_now) - window_wall_start; - if (wall_elapsed >= 1.0) { - const double sim_elapsed = static_cast(steps - window_step_start) * dt; - measured_rtf_.store(sim_elapsed / wall_elapsed, std::memory_order_relaxed); - window_wall_start = to_seconds(wall_now); - window_step_start = steps; + StepController* ctrl = controller_.load(std::memory_order_acquire); + if (ctrl != nullptr) { + s->mode = ctrl->mode(); + s->fall_state = ctrl->fall_state(); + s->getting_up = ctrl->getting_up(); + } + else { + s->mode = booster::RobotMode::PREPARE; + s->fall_state = booster::FallState::IS_READY; + s->getting_up = false; + } + + s->measured_rtf = measured_rtf_.load(std::memory_order_relaxed); + return s; + } + + void SimCore::physics_loop() { + const double dt = m_->opt.timestep; + const bool free_run = config_.rtf <= 0.0; + const double period = free_run ? 0.0 : dt / config_.rtf; + const auto publish_every = + static_cast(config_.state_publish_divisor > 0 ? config_.state_publish_divisor : 0); + + timespec deadline{}; + clock_gettime(CLOCK_MONOTONIC, &deadline); + + double window_wall_start = to_seconds(deadline); + uint64_t window_step_start = 0; + uint64_t steps = 0; + + while (running_.load(std::memory_order_acquire)) { + std::unique_ptr snapshot; + { + std::lock_guard lock(mutex_); + + StepController* ctrl = controller_.load(std::memory_order_acquire); + if (ctrl != nullptr) { + ctrl->step(m_, d_); + } + else { + pd_.apply(m_, d_, map_, ready_target_); + } + // Extra --robots copies have no controller; hold them at the ready pose. + for (const auto& em : extra_maps_) { + pd_.apply(m_, d_, em, ready_target_); + } + mj_step(m_, d_); + ++steps; + step_count_.store(steps, std::memory_order_relaxed); + + if (publish_every > 0 && steps % publish_every == 0) { + snapshot = make_snapshot(steps); + log_foot_state(); + } + } // release the mutex before emitting/pacing + + if (snapshot && on_state_) { + on_state_(std::move(snapshot)); + } + + if (!free_run) { + deadline = add_seconds(deadline, period); + timespec now_ts{}; + clock_gettime(CLOCK_MONOTONIC, &now_ts); + const double behind = to_seconds(now_ts) - to_seconds(deadline); + if (behind > config_.resync_threshold) { + deadline = now_ts; + dropped_deadlines_.fetch_add(1, std::memory_order_relaxed); + } + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &deadline, nullptr); + } + + timespec wall_now{}; + clock_gettime(CLOCK_MONOTONIC, &wall_now); + const double wall_elapsed = to_seconds(wall_now) - window_wall_start; + if (wall_elapsed >= 1.0) { + const double sim_elapsed = static_cast(steps - window_step_start) * dt; + measured_rtf_.store(sim_elapsed / wall_elapsed, std::memory_order_relaxed); + window_wall_start = to_seconds(wall_now); + window_step_start = steps; + } } } -} } // namespace k1sim diff --git a/mujoco/module/Simulation/src/SimCore.hpp b/mujoco/module/Simulation/src/SimCore.hpp index ba0c93c..0d10212 100644 --- a/mujoco/module/Simulation/src/SimCore.hpp +++ b/mujoco/module/Simulation/src/SimCore.hpp @@ -21,149 +21,165 @@ namespace k1sim { -// SimCore owns the mjModel/mjData and the dedicated physics thread. It is deliberately -// decoupled from NUClear (no Reactor/Environment dependency) so it can be driven directly -// by unit tests; module::Simulation is a thin NUClear wrapper around it (config loading, -// SimHandles/SimStateUpdate wiring, Startup/Shutdown lifecycle). -// -// Threading contract: mjData (d()) is only ever touched with mutex() held. The physics -// thread holds it for control+step+snapshot only; callers (e.g. the viewer) should acquire -// it briefly. set_controller()/controller() are lock-free (std::atomic) -// so a controller can be installed after start() without racing the physics thread. -class SimCore { -public: - using StateCallback = std::function)>; - - struct Config { - std::string model_path; // resolved via k1sim::config::resolve_path by the caller - std::string initial_keyframe = "ready"; // keyframe to spawn (and reset) into - double rtf = 1.0; // real-time factor; 0 = free-run (no pacing sleep) - int robots = 1; // total K1s; extras are attached copies PD-held at "ready" - int state_publish_divisor = 20; // physics steps per SimStateUpdate - double resync_threshold = 0.05; // seconds behind schedule before the deadline resyncs - - // Ground-contact override, applied to the geom named "floor" after the model loads. - // - // Without it the feet do NOT see the floor's declared friction: the foot box geom - // takes the MuJoCo default (1.0) while the robocup floor declares 0.8, and with equal - // geom priorities MuJoCo uses the element-wise MAX -- so every NUSim walk to date ran - // at mu = 1.0, grippier than the scene claims and grippier than anything the robot - // actually stands on. Enabling this sets the floor's priority to 1 so its numbers - // govern the contact, which is what the training scene does. - // - // This is the knob for the one confirmed hardware variable: the same policy and the - // same command hold up on carpet and progressively fall on synthetic grass. - struct Surface { - bool enabled = false; - double friction = 0.8; // sliding; mu = tan(slip angle) - double solref_timeconst = 0.02; // contact softness; keep >= 2 * model timestep - double solref_dampratio = 1.0; - } surface; - - // When non-empty, append one CSV row per published state to this path: per foot the - // total normal force, the centre of pressure in the foot's own frame, and the sole's - // pitch/roll. Centre of pressure is the tiptoe measurement -- a sole rolled onto its - // toe puts every contact at the front edge of the support polygon, which is exactly - // where the friction budget runs out. Off by default. - std::string foot_log_path; - - std::array kp{}; // PD fallback gains (gains.yaml) - std::array kd{}; // PD fallback gains (gains.yaml) - std::array ready_pose_fallback{}; // used only if the model has no - // "ready" keyframe + // SimCore owns the mjModel/mjData and the dedicated physics thread. It is deliberately + // decoupled from NUClear (no Reactor/Environment dependency) so it can be driven directly + // by unit tests; module::Simulation is a thin NUClear wrapper around it (config loading, + // SimHandles/SimStateUpdate wiring, Startup/Shutdown lifecycle). + // + // Threading contract: mjData (d()) is only ever touched with mutex() held. The physics + // thread holds it for control+step+snapshot only; callers (e.g. the viewer) should acquire + // it briefly. set_controller()/controller() are lock-free (std::atomic) + // so a controller can be installed after start() without racing the physics thread. + class SimCore { + public: + using StateCallback = std::function)>; + + struct Config { + std::string model_path; // resolved via k1sim::config::resolve_path by the caller + std::string initial_keyframe = "ready"; // keyframe to spawn (and reset) into + double rtf = 1.0; // real-time factor; 0 = free-run (no pacing sleep) + int robots = 1; // total K1s; extras are attached copies PD-held at "ready" + int state_publish_divisor = 20; // physics steps per SimStateUpdate + double resync_threshold = 0.05; // seconds behind schedule before the deadline resyncs + + // Ground-contact override, applied to the geom named "floor" after the model loads. + // + // Without it the feet do NOT see the floor's declared friction: the foot box geom + // takes the MuJoCo default (1.0) while the robocup floor declares 0.8, and with equal + // geom priorities MuJoCo uses the element-wise MAX -- so every NUSim walk to date ran + // at mu = 1.0, grippier than the scene claims and grippier than anything the robot + // actually stands on. Enabling this sets the floor's priority to 1 so its numbers + // govern the contact, which is what the training scene does. + // + // This is the knob for the one confirmed hardware variable: the same policy and the + // same command hold up on carpet and progressively fall on synthetic grass. + struct Surface { + bool enabled = false; + double friction = 0.8; // sliding; mu = tan(slip angle) + double solref_timeconst = 0.02; // contact softness; keep >= 2 * model timestep + double solref_dampratio = 1.0; + } surface; + + // When non-empty, append one CSV row per published state to this path: per foot the + // total normal force, the centre of pressure in the foot's own frame, and the sole's + // pitch/roll. Centre of pressure is the tiptoe measurement -- a sole rolled onto its + // toe puts every contact at the front edge of the support polygon, which is exactly + // where the friction budget runs out. Off by default. + std::string foot_log_path; + + std::array kp{}; // PD fallback gains (gains.yaml) + std::array kd{}; // PD fallback gains (gains.yaml) + std::array ready_pose_fallback{}; // used only if the model has no + // "ready" keyframe + }; + + explicit SimCore(Config config, StateCallback on_state = {}); + ~SimCore(); + + SimCore(const SimCore&) = delete; + SimCore& operator=(const SimCore&) = delete; + + // Loads the MJCF at config.model_path, builds the ModelMap, allocates mjData, and (if a + // keyframe named "ready" exists) resets to it and records its joint pose as the PD + // fallback target; otherwise the fallback target is config.ready_pose_fallback. + // Throws std::runtime_error on failure (missing file, missing joints/actuators, no free + // root joint — see ModelMap::build). Must be called exactly once, before start(). + void load_model(); + + const mjModel* model() const noexcept { + return m_; + } + mjData* data() const noexcept { + return d_; + } + const ModelMap& model_map() const noexcept { + return map_; + } + std::mutex& mutex() noexcept { + return mutex_; + } + std::atomic& measured_rtf() noexcept { + return measured_rtf_; + } + + // Thread-safe; may be called before or after start(), any number of times. + void set_controller(StepController* controller) noexcept { + controller_.store(controller, std::memory_order_release); + } + StepController* controller() const noexcept { + return controller_.load(std::memory_order_acquire); + } + + // Resets mjData back to the state load_model() established (the "ready" keyframe, or + // zeros if the model has none) — robot pose, ball, velocities, controls. Thread-safe + // (takes the sim mutex); callable while the physics thread runs. Does NOT reset the + // attached StepController: it keeps its mode and last commands, mirroring a real robot + // being picked up and placed back on its start point mid-program. + void reset(); + + // Spawns the physics thread. Requires load_model() to have already succeeded. Idempotent: + // calling start() again while already running is a no-op. + void start(); + + // Signals the physics thread to stop and joins it. Safe to call multiple times, or if the + // thread was never started. + void stop(); + + // Frees mjData/mjModel. Call after stop(). Safe to call multiple times. + void unload(); + + uint64_t step_count() const noexcept { + return step_count_.load(std::memory_order_relaxed); + } + uint64_t dropped_deadlines() const noexcept { + return dropped_deadlines_.load(std::memory_order_relaxed); + } + + private: + void physics_loop(); + // Applies Config::surface to the "floor" geom. No-op when the override is disabled or + // the scene has no geom called "floor". Called by load_model() before mj_makeData. + void apply_surface_override(); + // Appends one row to Config::foot_log_path. No-op when logging is off. Requires the + // caller to hold mutex_ (it reads d_->contact). + void log_foot_state(); + // Puts the extra --robots copies at their spawn slots in the ready pose; must run + // after every keyframe reset (whose zero-padding would pile them at the origin). + void place_extras(); + // Requires the caller to hold mutex_. Reads d_/map_/controller state into a fresh message. + std::unique_ptr make_snapshot(uint64_t steps) const; + + Config config_; + StateCallback on_state_; + + mjModel* m_ = nullptr; + mjData* d_ = nullptr; + int reset_key_ = -1; // keyframe id load_model() reset to; reused by reset() + ModelMap map_{}; + std::array ready_target_{}; + PdController pd_{}; + // Joint/actuator index maps for the extra --robots copies (root/sensor fields unused); + // the physics loop PD-holds each of them at ready_target_ every step. + std::vector extra_maps_; + + std::mutex mutex_; + std::atomic measured_rtf_{0.0}; + std::atomic controller_{nullptr}; + + std::atomic running_{false}; + std::thread thread_; + std::atomic step_count_{0}; + std::atomic dropped_deadlines_{0}; + + // Foot-contact CSV (Config::foot_log_path); nullptr when logging is off. + std::FILE* foot_log_ = nullptr; + int left_foot_body_id_ = -1; + int right_foot_body_id_ = -1; + + // Head_2, for SimStateUpdate::head (published as rt/head_pose); -1 if absent. + int head_body_id_ = -1; }; - explicit SimCore(Config config, StateCallback on_state = {}); - ~SimCore(); - - SimCore(const SimCore&) = delete; - SimCore& operator=(const SimCore&) = delete; - - // Loads the MJCF at config.model_path, builds the ModelMap, allocates mjData, and (if a - // keyframe named "ready" exists) resets to it and records its joint pose as the PD - // fallback target; otherwise the fallback target is config.ready_pose_fallback. - // Throws std::runtime_error on failure (missing file, missing joints/actuators, no free - // root joint — see ModelMap::build). Must be called exactly once, before start(). - void load_model(); - - const mjModel* model() const noexcept { return m_; } - mjData* data() const noexcept { return d_; } - const ModelMap& model_map() const noexcept { return map_; } - std::mutex& mutex() noexcept { return mutex_; } - std::atomic& measured_rtf() noexcept { return measured_rtf_; } - - // Thread-safe; may be called before or after start(), any number of times. - void set_controller(StepController* controller) noexcept { - controller_.store(controller, std::memory_order_release); - } - StepController* controller() const noexcept { return controller_.load(std::memory_order_acquire); } - - // Resets mjData back to the state load_model() established (the "ready" keyframe, or - // zeros if the model has none) — robot pose, ball, velocities, controls. Thread-safe - // (takes the sim mutex); callable while the physics thread runs. Does NOT reset the - // attached StepController: it keeps its mode and last commands, mirroring a real robot - // being picked up and placed back on its start point mid-program. - void reset(); - - // Spawns the physics thread. Requires load_model() to have already succeeded. Idempotent: - // calling start() again while already running is a no-op. - void start(); - - // Signals the physics thread to stop and joins it. Safe to call multiple times, or if the - // thread was never started. - void stop(); - - // Frees mjData/mjModel. Call after stop(). Safe to call multiple times. - void unload(); - - uint64_t step_count() const noexcept { return step_count_.load(std::memory_order_relaxed); } - uint64_t dropped_deadlines() const noexcept { return dropped_deadlines_.load(std::memory_order_relaxed); } - -private: - void physics_loop(); - // Applies Config::surface to the "floor" geom. No-op when the override is disabled or - // the scene has no geom called "floor". Called by load_model() before mj_makeData. - void apply_surface_override(); - // Appends one row to Config::foot_log_path. No-op when logging is off. Requires the - // caller to hold mutex_ (it reads d_->contact). - void log_foot_state(); - // Puts the extra --robots copies at their spawn slots in the ready pose; must run - // after every keyframe reset (whose zero-padding would pile them at the origin). - void place_extras(); - // Requires the caller to hold mutex_. Reads d_/map_/controller state into a fresh message. - std::unique_ptr make_snapshot(uint64_t steps) const; - - Config config_; - StateCallback on_state_; - - mjModel* m_ = nullptr; - mjData* d_ = nullptr; - int reset_key_ = -1; // keyframe id load_model() reset to; reused by reset() - ModelMap map_{}; - std::array ready_target_{}; - PdController pd_{}; - // Joint/actuator index maps for the extra --robots copies (root/sensor fields unused); - // the physics loop PD-holds each of them at ready_target_ every step. - std::vector extra_maps_; - - std::mutex mutex_; - std::atomic measured_rtf_{0.0}; - std::atomic controller_{nullptr}; - - std::atomic running_{false}; - std::thread thread_; - std::atomic step_count_{0}; - std::atomic dropped_deadlines_{0}; - - // Foot-contact CSV (Config::foot_log_path); nullptr when logging is off. - std::FILE* foot_log_ = nullptr; - int left_foot_body_id_ = -1; - int right_foot_body_id_ = -1; - - // Head_2, for SimStateUpdate::head (published as rt/head_pose); -1 if absent. - int head_body_id_ = -1; -}; - } // namespace k1sim #endif // K1SIM_MODULE_SIMULATION_SIMCORE_HPP diff --git a/mujoco/module/Simulation/src/Simulation.cpp b/mujoco/module/Simulation/src/Simulation.cpp index 3c3aa0e..a20de1e 100644 --- a/mujoco/module/Simulation/src/Simulation.cpp +++ b/mujoco/module/Simulation/src/Simulation.cpp @@ -13,115 +13,115 @@ namespace k1sim::module { -namespace { - -SimCore::Config build_sim_config() { - auto sim_cfg = config::load("simulation.yaml"); - auto gains_cfg = config::load("gains.yaml"); - - SimCore::Config cfg; - cfg.model_path = !cli().model.empty() ? cli().model : config::field_scene(sim_cfg, cli().field); - cfg.initial_keyframe = - !cli().keyframe.empty() ? cli().keyframe : sim_cfg["initial_keyframe"].as("ready"); - // CliOptions.rtf < 0 means "use config"; the config's real_time_factor may itself be 0 - // (free-run) — SimCore treats rtf <= 0 as free-run. - cfg.rtf = cli().rtf >= 0.0 ? cli().rtf : sim_cfg["real_time_factor"].as(1.0); - cfg.robots = cli().robots; - cfg.state_publish_divisor = sim_cfg["state_publish_divisor"].as(20); - cfg.resync_threshold = sim_cfg["resync_threshold"].as(0.05); - - const auto surface = sim_cfg["surface"]; - cfg.surface.enabled = surface["enabled"].as(false); - cfg.surface.friction = surface["friction"].as(0.8); - cfg.surface.solref_timeconst = surface["solref_timeconst"].as(0.02); - cfg.surface.solref_dampratio = surface["solref_dampratio"].as(1.0); - - cfg.foot_log_path = sim_cfg["foot_log"].as(""); - - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - cfg.kp[i] = gains_cfg["kp"][i].as(); - cfg.kd[i] = gains_cfg["kd"][i].as(); - cfg.ready_pose_fallback[i] = gains_cfg["ready_pose"][i].as(); - } - return cfg; -} - -} // namespace - -Simulation::Simulation(std::unique_ptr environment) : Reactor(std::move(environment)) { - - // Constructed here (not inside on) — see the header comment on sim_. - SimCore::Config sim_config = build_sim_config(); - const std::string scene = sim_config.model_path; - sim_ = std::make_unique(std::move(sim_config), - [this](std::unique_ptr state) { emit(state); }); - - on().then([this, scene] { - sim_->load_model(); - - auto handles = std::make_unique(); - handles->model = sim_->model(); - handles->data = sim_->data(); - handles->mutex = &sim_->mutex(); - handles->measured_rtf = &sim_->measured_rtf(); - emit(handles); - - log("Simulation ready (scene", - scene, - "— MuJoCo", - mj_versionString(), - "— nq:", - sim_->model()->nq, - "nu:", - sim_->model()->nu, - ") — starting physics thread (PD-to-ready fallback until a " - "controller attaches)"); - - // Start immediately with the PD fallback engaged; if Locomotion's ControllerHandle - // arrives it is swapped in atomically by the Trigger reaction below — race-free, no - // need to wait for it (Locomotion may not even be installed, e.g. in unit tests). - sim_->start(); - }); - - on>().then([this](const message::ControllerHandle& handle) { - sim_->set_controller(handle.controller); - log("Simulation: controller attached"); - }); - - // Viewer Backspace (or any other emitter): snap mjData back to the startup keyframe. - // Physics state only — the attached controller keeps its mode/commands (SimCore::reset). - on>().then([this] { - sim_->reset(); - log("Simulation: state reset to startup keyframe"); - }); - - // Base-pose heartbeat: one INFO line every ~5 s of sim time (updates arrive at - // 50 Hz), so headless runs show whether the robot is actually moving. - on>().then([this](const message::SimStateUpdate& s) { - if (s.sim_time >= next_pose_log_) { - next_pose_log_ = s.sim_time + 5.0; - log("Simulation: t =", - s.sim_time, - "s, base x =", - s.base.x, - "y =", - s.base.y, - "z =", - s.base.z, - ", mode =", - s.mode); + namespace { + + SimCore::Config build_sim_config() { + auto sim_cfg = config::load("simulation.yaml"); + auto gains_cfg = config::load("gains.yaml"); + + SimCore::Config cfg; + cfg.model_path = !cli().model.empty() ? cli().model : config::field_scene(sim_cfg, cli().field); + cfg.initial_keyframe = + !cli().keyframe.empty() ? cli().keyframe : sim_cfg["initial_keyframe"].as("ready"); + // CliOptions.rtf < 0 means "use config"; the config's real_time_factor may itself be 0 + // (free-run) — SimCore treats rtf <= 0 as free-run. + cfg.rtf = cli().rtf >= 0.0 ? cli().rtf : sim_cfg["real_time_factor"].as(1.0); + cfg.robots = cli().robots; + cfg.state_publish_divisor = sim_cfg["state_publish_divisor"].as(20); + cfg.resync_threshold = sim_cfg["resync_threshold"].as(0.05); + + const auto surface = sim_cfg["surface"]; + cfg.surface.enabled = surface["enabled"].as(false); + cfg.surface.friction = surface["friction"].as(0.8); + cfg.surface.solref_timeconst = surface["solref_timeconst"].as(0.02); + cfg.surface.solref_dampratio = surface["solref_dampratio"].as(1.0); + + cfg.foot_log_path = sim_cfg["foot_log"].as(""); + + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + cfg.kp[i] = gains_cfg["kp"][i].as(); + cfg.kd[i] = gains_cfg["kd"][i].as(); + cfg.ready_pose_fallback[i] = gains_cfg["ready_pose"][i].as(); + } + return cfg; } - }); - - on().then([this] { - log("Simulation shutting down (measured RTF", - sim_->measured_rtf().load(), - ", dropped deadlines:", - sim_->dropped_deadlines(), - ")"); - sim_->stop(); - sim_->unload(); - }); -} + + } // namespace + + Simulation::Simulation(std::unique_ptr environment) : Reactor(std::move(environment)) { + + // Constructed here (not inside on) — see the header comment on sim_. + SimCore::Config sim_config = build_sim_config(); + const std::string scene = sim_config.model_path; + sim_ = std::make_unique(std::move(sim_config), + [this](std::unique_ptr state) { emit(state); }); + + on().then([this, scene] { + sim_->load_model(); + + auto handles = std::make_unique(); + handles->model = sim_->model(); + handles->data = sim_->data(); + handles->mutex = &sim_->mutex(); + handles->measured_rtf = &sim_->measured_rtf(); + emit(handles); + + log("Simulation ready (scene", + scene, + "— MuJoCo", + mj_versionString(), + "— nq:", + sim_->model()->nq, + "nu:", + sim_->model()->nu, + ") — starting physics thread (PD-to-ready fallback until a " + "controller attaches)"); + + // Start immediately with the PD fallback engaged; if Locomotion's ControllerHandle + // arrives it is swapped in atomically by the Trigger reaction below — race-free, no + // need to wait for it (Locomotion may not even be installed, e.g. in unit tests). + sim_->start(); + }); + + on>().then([this](const message::ControllerHandle& handle) { + sim_->set_controller(handle.controller); + log("Simulation: controller attached"); + }); + + // Viewer Backspace (or any other emitter): snap mjData back to the startup keyframe. + // Physics state only — the attached controller keeps its mode/commands (SimCore::reset). + on>().then([this] { + sim_->reset(); + log("Simulation: state reset to startup keyframe"); + }); + + // Base-pose heartbeat: one INFO line every ~5 s of sim time (updates arrive at + // 50 Hz), so headless runs show whether the robot is actually moving. + on>().then([this](const message::SimStateUpdate& s) { + if (s.sim_time >= next_pose_log_) { + next_pose_log_ = s.sim_time + 5.0; + log("Simulation: t =", + s.sim_time, + "s, base x =", + s.base.x, + "y =", + s.base.y, + "z =", + s.base.z, + ", mode =", + s.mode); + } + }); + + on().then([this] { + log("Simulation shutting down (measured RTF", + sim_->measured_rtf().load(), + ", dropped deadlines:", + sim_->dropped_deadlines(), + ")"); + sim_->stop(); + sim_->unload(); + }); + } } // namespace k1sim::module diff --git a/mujoco/module/Simulation/src/Simulation.hpp b/mujoco/module/Simulation/src/Simulation.hpp index cd2f08d..3fad66f 100644 --- a/mujoco/module/Simulation/src/Simulation.hpp +++ b/mujoco/module/Simulation/src/Simulation.hpp @@ -8,22 +8,22 @@ namespace k1sim::module { -// Thin NUClear wrapper around SimCore: loads config, loads the model, emits SimHandles once -// (Startup) and SimStateUpdate at 50 Hz (from the physics thread), starts the physics thread -// immediately with the PD-to-ready fallback engaged, and atomically swaps in the -// Locomotion-provided StepController whenever a ControllerHandle arrives. -class Simulation : public NUClear::Reactor { -public: - explicit Simulation(std::unique_ptr environment); + // Thin NUClear wrapper around SimCore: loads config, loads the model, emits SimHandles once + // (Startup) and SimStateUpdate at 50 Hz (from the physics thread), starts the physics thread + // immediately with the PD-to-ready fallback engaged, and atomically swaps in the + // Locomotion-provided StepController whenever a ControllerHandle arrives. + class Simulation : public NUClear::Reactor { + public: + explicit Simulation(std::unique_ptr environment); -private: - // Constructed in the Reactor constructor (not inside on) so that its atomic - // controller_ member always exists before any reaction can run — on handlers - // across reactors are not guaranteed to be ordered/serialized by NUClear, so - // on> could otherwise race the construction of sim_. - std::unique_ptr sim_; - double next_pose_log_ = 0.0; // sim-time (s) of the next base-pose heartbeat log -}; + private: + // Constructed in the Reactor constructor (not inside on) so that its atomic + // controller_ member always exists before any reaction can run — on handlers + // across reactors are not guaranteed to be ordered/serialized by NUClear, so + // on> could otherwise race the construction of sim_. + std::unique_ptr sim_; + double next_pose_log_ = 0.0; // sim-time (s) of the next base-pose heartbeat log + }; } // namespace k1sim::module diff --git a/mujoco/module/Supervisor/CMakeLists.txt b/mujoco/module/Supervisor/CMakeLists.txt index 1688928..1d31577 100644 --- a/mujoco/module/Supervisor/CMakeLists.txt +++ b/mujoco/module/Supervisor/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(k1sim_module_supervisor STATIC src/Supervisor.cpp) target_link_libraries(k1sim_module_supervisor PUBLIC k1sim_shared) -# GameControllerPacket.hpp / SupervisorPlacement.hpp / SupervisorConfig.hpp / -# SupervisorLogic.hpp are header-only (no extra sources here) so -# test/unit/test_supervisor.cpp can use them without linking this library — -# see that test CMakeLists' comment on why it doesn't link per-module libs. +# GameControllerPacket.hpp / SupervisorPlacement.hpp / SupervisorConfig.hpp / SupervisorLogic.hpp are header-only (no +# extra sources here) so test/unit/test_supervisor.cpp can use them without linking this library — see that test +# CMakeLists' comment on why it doesn't link per-module libs. diff --git a/mujoco/module/Supervisor/src/GameControllerPacket.hpp b/mujoco/module/Supervisor/src/GameControllerPacket.hpp index 825b70a..499f20b 100644 --- a/mujoco/module/Supervisor/src/GameControllerPacket.hpp +++ b/mujoco/module/Supervisor/src/GameControllerPacket.hpp @@ -23,154 +23,154 @@ // reinterpret_cast parsing to work — see try_parse() at the bottom. namespace k1sim::module::supervisor::gc { -inline constexpr std::size_t MAX_NUM_PLAYERS = 20; -inline constexpr std::array RECEIVE_HEADER = {'R', 'G', 'm', 'e'}; -inline constexpr std::uint8_t SUPPORTED_VERSION = 20; + inline constexpr std::size_t MAX_NUM_PLAYERS = 20; + inline constexpr std::array RECEIVE_HEADER = {'R', 'G', 'm', 'e'}; + inline constexpr std::uint8_t SUPPORTED_VERSION = 20; #pragma pack(push, 1) -enum class State : std::uint8_t { INITIAL = 0, READY = 1, SET = 2, PLAYING = 3, FINISHED = 4 }; - -enum class GamePhase : std::uint8_t { - NORMAL = 0, - PENALTY_SHOOTOUT = 1, - EXTRA_TIME = 2, - TIMEOUT = 3, -}; - -enum class SetPlay : std::uint8_t { - NONE = 0, - DIRECT_FREE_KICK = 1, - INDIRECT_FREE_KICK = 2, - PENALTY_KICK = 3, - THROW_IN = 4, - GOAL_KICK = 5, - CORNER_KICK = 6, -}; - -enum class TeamColour : std::uint8_t { - BLUE = 0, - RED = 1, - YELLOW = 2, - BLACK = 3, - WHITE = 4, - GREEN = 5, - ORANGE = 6, - PURPLE = 7, - BROWN = 8, - GRAY = 9, -}; - -// Per-player penalty state. UNPENALISED is the only "in play" value; every -// other value means the player is currently sitting out for some reason. -enum class PenaltyState : std::uint8_t { - UNPENALISED = 0, - ILLEGAL_POSITIONING = 1, - MOTION_IN_SET = 2, - MOTION_IN_STOP = 3, - LOCAL_GAME_STUCK = 4, - INCAPABLE_ROBOT = 5, - PICK_UP = 6, - BALL_HOLDING = 7, - LEAVING_THE_FIELD = 8, - PLAYING_WITH_ARMS_HANDS = 9, - PLAYER_PUSHING = 10, - CAUTIONED = 11, - SENT_OFF = 12, - SUBSTITUTE = 13, -}; - -struct Robot { - PenaltyState penalty_state; // penalty state of the player - std::uint8_t penalised_time_left; // estimate of time till unpenalised (seconds) - std::uint8_t cautions; // number of cautions (yellow cards) -}; - -struct Team { - std::uint8_t team_id; // unique team number - TeamColour field_player_colour; // colour of the field players - TeamColour goalkeeper_colour; // colour of the goalkeeper - std::uint8_t goalkeeper; // player number of the goalkeeper (0-MAX_NUM_PLAYERS) - std::uint8_t score; // team's score - std::uint8_t penalty_shot; // penalty shot counter - std::uint16_t single_shots; // bits represent penalty shot success - std::uint16_t message_budget; // remaining team message budget - std::array players; -}; - -struct GameControllerPacket { - std::array header; // must equal RECEIVE_HEADER ('R','G','m','e') - std::uint8_t version; // must equal SUPPORTED_VERSION (20) - std::uint8_t packet_number; - std::uint8_t players_per_team; - std::uint8_t competition_type; - std::uint8_t stopped; // 1 = play currently stopped - GamePhase game_phase; - State state; - SetPlay set_play; - bool first_half; // true = first half - std::uint8_t kicking_team; // team_id of the team with the next kickoff/free kick - std::int16_t secs_remaining; - std::int16_t secondary_time; - std::array teams; -}; + enum class State : std::uint8_t { INITIAL = 0, READY = 1, SET = 2, PLAYING = 3, FINISHED = 4 }; + + enum class GamePhase : std::uint8_t { + NORMAL = 0, + PENALTY_SHOOTOUT = 1, + EXTRA_TIME = 2, + TIMEOUT = 3, + }; + + enum class SetPlay : std::uint8_t { + NONE = 0, + DIRECT_FREE_KICK = 1, + INDIRECT_FREE_KICK = 2, + PENALTY_KICK = 3, + THROW_IN = 4, + GOAL_KICK = 5, + CORNER_KICK = 6, + }; + + enum class TeamColour : std::uint8_t { + BLUE = 0, + RED = 1, + YELLOW = 2, + BLACK = 3, + WHITE = 4, + GREEN = 5, + ORANGE = 6, + PURPLE = 7, + BROWN = 8, + GRAY = 9, + }; + + // Per-player penalty state. UNPENALISED is the only "in play" value; every + // other value means the player is currently sitting out for some reason. + enum class PenaltyState : std::uint8_t { + UNPENALISED = 0, + ILLEGAL_POSITIONING = 1, + MOTION_IN_SET = 2, + MOTION_IN_STOP = 3, + LOCAL_GAME_STUCK = 4, + INCAPABLE_ROBOT = 5, + PICK_UP = 6, + BALL_HOLDING = 7, + LEAVING_THE_FIELD = 8, + PLAYING_WITH_ARMS_HANDS = 9, + PLAYER_PUSHING = 10, + CAUTIONED = 11, + SENT_OFF = 12, + SUBSTITUTE = 13, + }; + + struct Robot { + PenaltyState penalty_state; // penalty state of the player + std::uint8_t penalised_time_left; // estimate of time till unpenalised (seconds) + std::uint8_t cautions; // number of cautions (yellow cards) + }; + + struct Team { + std::uint8_t team_id; // unique team number + TeamColour field_player_colour; // colour of the field players + TeamColour goalkeeper_colour; // colour of the goalkeeper + std::uint8_t goalkeeper; // player number of the goalkeeper (0-MAX_NUM_PLAYERS) + std::uint8_t score; // team's score + std::uint8_t penalty_shot; // penalty shot counter + std::uint16_t single_shots; // bits represent penalty shot success + std::uint16_t message_budget; // remaining team message budget + std::array players; + }; + + struct GameControllerPacket { + std::array header; // must equal RECEIVE_HEADER ('R','G','m','e') + std::uint8_t version; // must equal SUPPORTED_VERSION (20) + std::uint8_t packet_number; + std::uint8_t players_per_team; + std::uint8_t competition_type; + std::uint8_t stopped; // 1 = play currently stopped + GamePhase game_phase; + State state; + SetPlay set_play; + bool first_half; // true = first half + std::uint8_t kicking_team; // team_id of the team with the next kickoff/free kick + std::int16_t secs_remaining; + std::int16_t secondary_time; + std::array teams; + }; #pragma pack(pop) -inline const char* state_name(State s) { - switch (s) { - case State::INITIAL: return "INITIAL"; - case State::READY: return "READY"; - case State::SET: return "SET"; - case State::PLAYING: return "PLAYING"; - case State::FINISHED: return "FINISHED"; - default: return "UNKNOWN"; + inline const char* state_name(State s) { + switch (s) { + case State::INITIAL: return "INITIAL"; + case State::READY: return "READY"; + case State::SET: return "SET"; + case State::PLAYING: return "PLAYING"; + case State::FINISHED: return "FINISHED"; + default: return "UNKNOWN"; + } } -} - -inline const char* penalty_state_name(PenaltyState p) { - switch (p) { - case PenaltyState::UNPENALISED: return "UNPENALISED"; - case PenaltyState::ILLEGAL_POSITIONING: return "ILLEGAL_POSITIONING"; - case PenaltyState::MOTION_IN_SET: return "MOTION_IN_SET"; - case PenaltyState::MOTION_IN_STOP: return "MOTION_IN_STOP"; - case PenaltyState::LOCAL_GAME_STUCK: return "LOCAL_GAME_STUCK"; - case PenaltyState::INCAPABLE_ROBOT: return "INCAPABLE_ROBOT"; - case PenaltyState::PICK_UP: return "PICK_UP"; - case PenaltyState::BALL_HOLDING: return "BALL_HOLDING"; - case PenaltyState::LEAVING_THE_FIELD: return "LEAVING_THE_FIELD"; - case PenaltyState::PLAYING_WITH_ARMS_HANDS: return "PLAYING_WITH_ARMS_HANDS"; - case PenaltyState::PLAYER_PUSHING: return "PLAYER_PUSHING"; - case PenaltyState::CAUTIONED: return "CAUTIONED"; - case PenaltyState::SENT_OFF: return "SENT_OFF"; - case PenaltyState::SUBSTITUTE: return "SUBSTITUTE"; - default: return "UNKNOWN"; - } -} - -// A State value that never equals any real State — used as the "no packet -// seen yet" sentinel so the very first real packet is always treated as a -// transition (mirrors NUbots' GameController::reset_state(), which seeds -// packet.state = static_cast(-1) for the same reason). -inline constexpr State UNKNOWN_STATE = static_cast(0xFF); - -// Parses a raw UDP payload into a GameControllerPacket. Returns false (leaves -// out untouched) if the payload is too short, or its header/version don't -// match — callers should treat that as "not a GameController packet" and -// ignore it silently, not as an error (harmless traffic on the port, or a GC -// version we don't speak, is expected and not exceptional). -inline bool try_parse(const std::uint8_t* data, std::size_t len, GameControllerPacket& out) { - if (data == nullptr || len < sizeof(GameControllerPacket)) { - return false; + + inline const char* penalty_state_name(PenaltyState p) { + switch (p) { + case PenaltyState::UNPENALISED: return "UNPENALISED"; + case PenaltyState::ILLEGAL_POSITIONING: return "ILLEGAL_POSITIONING"; + case PenaltyState::MOTION_IN_SET: return "MOTION_IN_SET"; + case PenaltyState::MOTION_IN_STOP: return "MOTION_IN_STOP"; + case PenaltyState::LOCAL_GAME_STUCK: return "LOCAL_GAME_STUCK"; + case PenaltyState::INCAPABLE_ROBOT: return "INCAPABLE_ROBOT"; + case PenaltyState::PICK_UP: return "PICK_UP"; + case PenaltyState::BALL_HOLDING: return "BALL_HOLDING"; + case PenaltyState::LEAVING_THE_FIELD: return "LEAVING_THE_FIELD"; + case PenaltyState::PLAYING_WITH_ARMS_HANDS: return "PLAYING_WITH_ARMS_HANDS"; + case PenaltyState::PLAYER_PUSHING: return "PLAYER_PUSHING"; + case PenaltyState::CAUTIONED: return "CAUTIONED"; + case PenaltyState::SENT_OFF: return "SENT_OFF"; + case PenaltyState::SUBSTITUTE: return "SUBSTITUTE"; + default: return "UNKNOWN"; + } } - GameControllerPacket packet; // NOLINT — POD, filled by memcpy below - std::memcpy(&packet, data, sizeof(GameControllerPacket)); - if (packet.header != RECEIVE_HEADER || packet.version != SUPPORTED_VERSION) { - return false; + + // A State value that never equals any real State — used as the "no packet + // seen yet" sentinel so the very first real packet is always treated as a + // transition (mirrors NUbots' GameController::reset_state(), which seeds + // packet.state = static_cast(-1) for the same reason). + inline constexpr State UNKNOWN_STATE = static_cast(0xFF); + + // Parses a raw UDP payload into a GameControllerPacket. Returns false (leaves + // out untouched) if the payload is too short, or its header/version don't + // match — callers should treat that as "not a GameController packet" and + // ignore it silently, not as an error (harmless traffic on the port, or a GC + // version we don't speak, is expected and not exceptional). + inline bool try_parse(const std::uint8_t* data, std::size_t len, GameControllerPacket& out) { + if (data == nullptr || len < sizeof(GameControllerPacket)) { + return false; + } + GameControllerPacket packet; // NOLINT — POD, filled by memcpy below + std::memcpy(&packet, data, sizeof(GameControllerPacket)); + if (packet.header != RECEIVE_HEADER || packet.version != SUPPORTED_VERSION) { + return false; + } + out = packet; + return true; } - out = packet; - return true; -} } // namespace k1sim::module::supervisor::gc diff --git a/mujoco/module/Supervisor/src/Supervisor.cpp b/mujoco/module/Supervisor/src/Supervisor.cpp index 227a94d..dfc94c8 100644 --- a/mujoco/module/Supervisor/src/Supervisor.cpp +++ b/mujoco/module/Supervisor/src/Supervisor.cpp @@ -9,102 +9,105 @@ namespace k1sim::module { -namespace gc = k1sim::module::supervisor::gc; -using k1sim::module::supervisor::SupervisorConfig; -using k1sim::module::supervisor::SupervisorLogic; - -namespace { - -SupervisorConfig build_config() { - return k1sim::module::supervisor::load_config(config::load("supervisor.yaml")); -} - -} // namespace - -Supervisor::Supervisor(std::unique_ptr environment) : Reactor(std::move(environment)) { - - // Loaded eagerly (constructor body, not Startup) so the port number is - // known before the on reaction below is installed — mirrors - // module::Simulation's build_sim_config(), the established pattern in - // this tree for config that has to be ready before reactions bind - // (this port isn't hot-reloadable; NUClear's on extension - // isn't used anywhere in this MuJoCo port, see shared/util/Config.hpp). - SupervisorConfig cfg = build_config(); - const bool enabled = cfg.enabled; - const int gc_port = cfg.gc_port; - logic_ = std::make_unique(std::move(cfg)); - - on>().then([this](const message::SimHandles& handles) { - model_.store(handles.model, std::memory_order_release); - data_.store(handles.data, std::memory_order_release); - sim_mutex_.store(handles.mutex, std::memory_order_release); - }); - - if (!enabled) { - on().then( - [this] { log("Supervisor disabled (supervisor.yaml: enabled: false)"); }); - on().then([this] { log("Supervisor shutting down"); }); - return; - } + namespace gc = k1sim::module::supervisor::gc; + using k1sim::module::supervisor::SupervisorConfig; + using k1sim::module::supervisor::SupervisorLogic; - // Plain on (unicast-style bind to any local address) rather than - // on: the latter only accepts datagrams whose - // *destination* address is itself a broadcast address (255.255.255.255, - // or an interface's configured broadcast address) — real GameController - // traffic qualifies, but a directly-addressed test packet to - // 127.0.0.1:gc_port (the loopback interface has no broadcast address at - // all on Linux) would silently never arrive, which would make this - // module untestable/unverifiable without a real LAN. Binding to - // INADDR_ANY still receives genuine LAN broadcasts addressed to this - // host, so real GameController traffic works identically either way. - // Binding can throw if the port is already taken (another sim instance, or a - // real GameController listener on this host). That must not crash the whole - // sim — degrade to "no supervisor" and log, matching the no-GC idle contract. - try { - on(gc_port).then([this](const UDP::Packet& packet) { - gc::GameControllerPacket parsed{}; - if (!gc::try_parse(packet.payload.data(), packet.payload.size(), parsed)) { - // Not a (recognised-version) GameController packet — could be - // unrelated traffic on this port, or a header/version mismatch. - // Silently ignored, per "no GC on the network" being a normal, - // error-free idle state, not a fault. - return; + namespace { + + SupervisorConfig build_config() { + return k1sim::module::supervisor::load_config(config::load("supervisor.yaml")); } - const mjModel* m = model_.load(std::memory_order_acquire); - mjData* d = data_.load(std::memory_order_acquire); - std::mutex* sim_mutex = sim_mutex_.load(std::memory_order_acquire); - if (m == nullptr || d == nullptr || sim_mutex == nullptr) { - log("GameController packet received before the sim model was ready — ignoring"); + } // namespace + + Supervisor::Supervisor(std::unique_ptr environment) : Reactor(std::move(environment)) { + + // Loaded eagerly (constructor body, not Startup) so the port number is + // known before the on reaction below is installed — mirrors + // module::Simulation's build_sim_config(), the established pattern in + // this tree for config that has to be ready before reactions bind + // (this port isn't hot-reloadable; NUClear's on extension + // isn't used anywhere in this MuJoCo port, see shared/util/Config.hpp). + SupervisorConfig cfg = build_config(); + const bool enabled = cfg.enabled; + const int gc_port = cfg.gc_port; + logic_ = std::make_unique(std::move(cfg)); + + on>().then([this](const message::SimHandles& handles) { + model_.store(handles.model, std::memory_order_release); + data_.store(handles.data, std::memory_order_release); + sim_mutex_.store(handles.mutex, std::memory_order_release); + }); + + if (!enabled) { + on().then( + [this] { log("Supervisor disabled (supervisor.yaml: enabled: false)"); }); + on().then([this] { log("Supervisor shutting down"); }); return; } - std::vector actions; - { - std::lock_guard lock(*sim_mutex); - actions = logic_->process(m, d, parsed); + // Plain on (unicast-style bind to any local address) rather than + // on: the latter only accepts datagrams whose + // *destination* address is itself a broadcast address (255.255.255.255, + // or an interface's configured broadcast address) — real GameController + // traffic qualifies, but a directly-addressed test packet to + // 127.0.0.1:gc_port (the loopback interface has no broadcast address at + // all on Linux) would silently never arrive, which would make this + // module untestable/unverifiable without a real LAN. Binding to + // INADDR_ANY still receives genuine LAN broadcasts addressed to this + // host, so real GameController traffic works identically either way. + // Binding can throw if the port is already taken (another sim instance, or a + // real GameController listener on this host). That must not crash the whole + // sim — degrade to "no supervisor" and log, matching the no-GC idle contract. + try { + on(gc_port).then([this](const UDP::Packet& packet) { + gc::GameControllerPacket parsed{}; + if (!gc::try_parse(packet.payload.data(), packet.payload.size(), parsed)) { + // Not a (recognised-version) GameController packet — could be + // unrelated traffic on this port, or a header/version mismatch. + // Silently ignored, per "no GC on the network" being a normal, + // error-free idle state, not a fault. + return; + } + + const mjModel* m = model_.load(std::memory_order_acquire); + mjData* d = data_.load(std::memory_order_acquire); + std::mutex* sim_mutex = sim_mutex_.load(std::memory_order_acquire); + if (m == nullptr || d == nullptr || sim_mutex == nullptr) { + log( + "GameController packet received before the sim model was ready — ignoring"); + return; + } + + std::vector actions; + { + std::lock_guard lock(*sim_mutex); + actions = logic_->process(m, d, parsed); + } + for (const auto& action : actions) { + if (action.level == SupervisorLogic::Action::Level::WARN) { + log(action.message.c_str()); + } + else { + log(action.message.c_str()); + } + } + }); } - for (const auto& action : actions) { - if (action.level == SupervisorLogic::Action::Level::WARN) { - log(action.message.c_str()); - } - else { - log(action.message.c_str()); - } + catch (const std::exception& e) { + log("Supervisor: could not bind UDP port", + gc_port, + "(", + e.what(), + ") — running without GameController placement"); } - }); - } - catch (const std::exception& e) { - log("Supervisor: could not bind UDP port", gc_port, "(", e.what(), - ") — running without GameController placement"); - } - - on().then([this, gc_port] { - log("Supervisor ready — waiting for GameController broadcasts on port", gc_port); - }); + on().then([this, gc_port] { + log("Supervisor ready — waiting for GameController broadcasts on port", gc_port); + }); - on().then([this] { log("Supervisor shutting down"); }); -} + on().then([this] { log("Supervisor shutting down"); }); + } } // namespace k1sim::module diff --git a/mujoco/module/Supervisor/src/Supervisor.hpp b/mujoco/module/Supervisor/src/Supervisor.hpp index 16fb99f..99fb9e2 100644 --- a/mujoco/module/Supervisor/src/Supervisor.hpp +++ b/mujoco/module/Supervisor/src/Supervisor.hpp @@ -11,36 +11,36 @@ namespace k1sim::module { -// Listens to the RoboCup GameController (UDP broadcast, port 3838 by default -// — see config/supervisor.yaml) and places physics bodies per game phase -// (ball to centre on kickoff, penalised robots off to the side line, etc.) — -// the sim-side supervisor role Webots used to provide. NUbots hears the -// GameController directly and independently over its own socket; this module -// never replies to it, it only watches the broadcast and moves bodies. A -// no-op (idle, no errors) whenever no GameController is on the network. -class Supervisor : public NUClear::Reactor { -public: - explicit Supervisor(std::unique_ptr environment); - -private: - // Decision logic (packet parsing + diffing + placement) — NUClear-free, - // see SupervisorLogic.hpp. Owned via pointer only so it can be - // constructed after config load, inside this constructor's body. - std::unique_ptr logic_; - - // Cached from message::SimHandles (Trigger fires once, after the model - // loads — see module::Simulation). Set on whichever thread pool worker - // runs that reaction and read from whichever worker runs the UDP - // reaction; both can run concurrently (neither is MainThread — this - // module never touches GL), so these are atomics rather than the plain - // pointers Viewer.cpp uses (Viewer pins both sides to MainThread instead, - // which isn't appropriate for a UDP listener). The mjData contents behind - // them are separately guarded by *sim_mutex_ while in use, exactly like - // every other module that touches mjData outside the physics thread. - std::atomic model_{nullptr}; - std::atomic data_{nullptr}; - std::atomic sim_mutex_{nullptr}; -}; + // Listens to the RoboCup GameController (UDP broadcast, port 3838 by default + // — see config/supervisor.yaml) and places physics bodies per game phase + // (ball to centre on kickoff, penalised robots off to the side line, etc.) — + // the sim-side supervisor role Webots used to provide. NUbots hears the + // GameController directly and independently over its own socket; this module + // never replies to it, it only watches the broadcast and moves bodies. A + // no-op (idle, no errors) whenever no GameController is on the network. + class Supervisor : public NUClear::Reactor { + public: + explicit Supervisor(std::unique_ptr environment); + + private: + // Decision logic (packet parsing + diffing + placement) — NUClear-free, + // see SupervisorLogic.hpp. Owned via pointer only so it can be + // constructed after config load, inside this constructor's body. + std::unique_ptr logic_; + + // Cached from message::SimHandles (Trigger fires once, after the model + // loads — see module::Simulation). Set on whichever thread pool worker + // runs that reaction and read from whichever worker runs the UDP + // reaction; both can run concurrently (neither is MainThread — this + // module never touches GL), so these are atomics rather than the plain + // pointers Viewer.cpp uses (Viewer pins both sides to MainThread instead, + // which isn't appropriate for a UDP listener). The mjData contents behind + // them are separately guarded by *sim_mutex_ while in use, exactly like + // every other module that touches mjData outside the physics thread. + std::atomic model_{nullptr}; + std::atomic data_{nullptr}; + std::atomic sim_mutex_{nullptr}; + }; } // namespace k1sim::module diff --git a/mujoco/module/Supervisor/src/SupervisorConfig.hpp b/mujoco/module/Supervisor/src/SupervisorConfig.hpp index 2387316..43ab7ca 100644 --- a/mujoco/module/Supervisor/src/SupervisorConfig.hpp +++ b/mujoco/module/Supervisor/src/SupervisorConfig.hpp @@ -12,102 +12,102 @@ // doesn't use NUClear's on hot-reload extension anywhere). namespace k1sim::module::supervisor { -// A flat-ground standing pose: world (x, y, z) plus yaw about +Z (radians). -struct Pose3 { - double x = 0.0; - double y = 0.0; - double z = 0.0; - double yaw = 0.0; -}; - -struct BallConfig { - bool enabled = true; - std::string body = "ball"; - std::string geom = "ball"; - double x = 0.0; - double y = 0.0; - // z < 0 means "rest the ball on the floor at its own geom radius"; - // only set this explicitly to override that. - double z = -1.0; -}; - -// One managed robot body. team_id/team_index resolve which of the packet's -// two Team entries governs this robot: if team_id >= 0 it is matched against -// GameControllerPacket::Team::team_id (like NUbots' own GameController -// resolves "our" team); otherwise team_index (0 or 1) just picks a packet -// slot positionally, which is all local/single-robot testing needs before a -// real competition team_id is configured. player_id is 1-based, indexing -// that team's players[] array (RoboCup convention: player 1..N). -struct RobotConfig { - std::string body; - int team_id = -1; - int team_index = 0; - int player_id = 1; - Pose3 home_pose; // where an unpenalised robot belongs (its own half) - Pose3 penalty_pose; // where a penalised robot is moved (side line) -}; - -struct SupervisorConfig { - bool enabled = true; - int gc_port = 3838; - - // Extra ball-centre resets beyond the core kickoff rule (entering READY, - // or entering PLAYING) — see SupervisorLogic::process. All optional. - bool reset_ball_on_finished = true; - bool reset_ball_on_goal = true; - bool reset_ball_on_half_change = true; - - BallConfig ball; - std::vector robots; -}; - -inline Pose3 load_pose(const YAML::Node& node, const Pose3& fallback = Pose3{}) { - if (!node) { - return fallback; - } - Pose3 pose; - pose.x = node["x"].as(fallback.x); - pose.y = node["y"].as(fallback.y); - pose.z = node["z"].as(fallback.z); - pose.yaw = node["yaw"].as(fallback.yaw); - return pose; -} - -inline SupervisorConfig load_config(const YAML::Node& root) { - SupervisorConfig cfg; - cfg.enabled = root["enabled"].as(cfg.enabled); - cfg.gc_port = root["gc_port"].as(cfg.gc_port); - - if (const auto& reset = root["reset_ball_on"]) { - cfg.reset_ball_on_finished = reset["finished"].as(cfg.reset_ball_on_finished); - cfg.reset_ball_on_goal = reset["goal"].as(cfg.reset_ball_on_goal); - cfg.reset_ball_on_half_change = reset["half_change"].as(cfg.reset_ball_on_half_change); + // A flat-ground standing pose: world (x, y, z) plus yaw about +Z (radians). + struct Pose3 { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double yaw = 0.0; + }; + + struct BallConfig { + bool enabled = true; + std::string body = "ball"; + std::string geom = "ball"; + double x = 0.0; + double y = 0.0; + // z < 0 means "rest the ball on the floor at its own geom radius"; + // only set this explicitly to override that. + double z = -1.0; + }; + + // One managed robot body. team_id/team_index resolve which of the packet's + // two Team entries governs this robot: if team_id >= 0 it is matched against + // GameControllerPacket::Team::team_id (like NUbots' own GameController + // resolves "our" team); otherwise team_index (0 or 1) just picks a packet + // slot positionally, which is all local/single-robot testing needs before a + // real competition team_id is configured. player_id is 1-based, indexing + // that team's players[] array (RoboCup convention: player 1..N). + struct RobotConfig { + std::string body; + int team_id = -1; + int team_index = 0; + int player_id = 1; + Pose3 home_pose; // where an unpenalised robot belongs (its own half) + Pose3 penalty_pose; // where a penalised robot is moved (side line) + }; + + struct SupervisorConfig { + bool enabled = true; + int gc_port = 3838; + + // Extra ball-centre resets beyond the core kickoff rule (entering READY, + // or entering PLAYING) — see SupervisorLogic::process. All optional. + bool reset_ball_on_finished = true; + bool reset_ball_on_goal = true; + bool reset_ball_on_half_change = true; + + BallConfig ball; + std::vector robots; + }; + + inline Pose3 load_pose(const YAML::Node& node, const Pose3& fallback = Pose3{}) { + if (!node) { + return fallback; + } + Pose3 pose; + pose.x = node["x"].as(fallback.x); + pose.y = node["y"].as(fallback.y); + pose.z = node["z"].as(fallback.z); + pose.yaw = node["yaw"].as(fallback.yaw); + return pose; } - if (const auto& ball = root["ball"]) { - cfg.ball.enabled = ball["enabled"].as(cfg.ball.enabled); - cfg.ball.body = ball["body"].as(cfg.ball.body); - cfg.ball.geom = ball["geom"].as(cfg.ball.geom); - cfg.ball.x = ball["x"].as(cfg.ball.x); - cfg.ball.y = ball["y"].as(cfg.ball.y); - cfg.ball.z = ball["z"].as(cfg.ball.z); - } + inline SupervisorConfig load_config(const YAML::Node& root) { + SupervisorConfig cfg; + cfg.enabled = root["enabled"].as(cfg.enabled); + cfg.gc_port = root["gc_port"].as(cfg.gc_port); - if (const auto& robots = root["robots"]) { - for (const auto& r : robots) { - RobotConfig rc; - rc.body = r["body"].as(); - rc.team_id = r["team_id"].as(rc.team_id); - rc.team_index = r["team_index"].as(rc.team_index); - rc.player_id = r["player_id"].as(rc.player_id); - rc.home_pose = load_pose(r["home_pose"]); - rc.penalty_pose = load_pose(r["penalty_pose"]); - cfg.robots.push_back(rc); + if (const auto& reset = root["reset_ball_on"]) { + cfg.reset_ball_on_finished = reset["finished"].as(cfg.reset_ball_on_finished); + cfg.reset_ball_on_goal = reset["goal"].as(cfg.reset_ball_on_goal); + cfg.reset_ball_on_half_change = reset["half_change"].as(cfg.reset_ball_on_half_change); } - } - return cfg; -} + if (const auto& ball = root["ball"]) { + cfg.ball.enabled = ball["enabled"].as(cfg.ball.enabled); + cfg.ball.body = ball["body"].as(cfg.ball.body); + cfg.ball.geom = ball["geom"].as(cfg.ball.geom); + cfg.ball.x = ball["x"].as(cfg.ball.x); + cfg.ball.y = ball["y"].as(cfg.ball.y); + cfg.ball.z = ball["z"].as(cfg.ball.z); + } + + if (const auto& robots = root["robots"]) { + for (const auto& r : robots) { + RobotConfig rc; + rc.body = r["body"].as(); + rc.team_id = r["team_id"].as(rc.team_id); + rc.team_index = r["team_index"].as(rc.team_index); + rc.player_id = r["player_id"].as(rc.player_id); + rc.home_pose = load_pose(r["home_pose"]); + rc.penalty_pose = load_pose(r["penalty_pose"]); + cfg.robots.push_back(rc); + } + } + + return cfg; + } } // namespace k1sim::module::supervisor diff --git a/mujoco/module/Supervisor/src/SupervisorLogic.hpp b/mujoco/module/Supervisor/src/SupervisorLogic.hpp index efb6735..b574b1e 100644 --- a/mujoco/module/Supervisor/src/SupervisorLogic.hpp +++ b/mujoco/module/Supervisor/src/SupervisorLogic.hpp @@ -21,190 +21,196 @@ // returned Actions to its own log<>() calls. namespace k1sim::module::supervisor { -class SupervisorLogic { -public: - explicit SupervisorLogic(SupervisorConfig cfg) : cfg_(std::move(cfg)) {} - - struct Action { - enum class Level { INFO, WARN } level = Level::INFO; - std::string message; - }; - - // Diffs `pkt` against the last packet seen (or, on the very first call, - // against the same "nothing has happened yet" baseline NUbots' - // GameController::reset_state() bootstraps: an unknown/sentinel State so - // any real State counts as a transition, and UNPENALISED for every - // player), applies whatever body placements that diff implies directly to - // `d`, and returns a log of what happened (empty if nothing did). - // - // Caller must hold the sim mutex (message::SimHandles::mutex) for the - // duration of this call — it writes mjData qpos/qvel with no locking of - // its own, same contract as the SupervisorPlacement.hpp functions it - // calls. - std::vector process(const mjModel* m, mjData* d, const gc::GameControllerPacket& pkt) { - std::vector actions; - - const gc::State old_state = prev_ ? prev_->state : gc::UNKNOWN_STATE; - const gc::State new_state = pkt.state; - const bool state_changed = new_state != old_state; - - if (state_changed) { - actions.push_back({Action::Level::INFO, std::string("state -> ") + gc::state_name(new_state)}); - } + class SupervisorLogic { + public: + explicit SupervisorLogic(SupervisorConfig cfg) : cfg_(std::move(cfg)) {} + + struct Action { + enum class Level { INFO, WARN } level = Level::INFO; + std::string message; + }; + + // Diffs `pkt` against the last packet seen (or, on the very first call, + // against the same "nothing has happened yet" baseline NUbots' + // GameController::reset_state() bootstraps: an unknown/sentinel State so + // any real State counts as a transition, and UNPENALISED for every + // player), applies whatever body placements that diff implies directly to + // `d`, and returns a log of what happened (empty if nothing did). + // + // Caller must hold the sim mutex (message::SimHandles::mutex) for the + // duration of this call — it writes mjData qpos/qvel with no locking of + // its own, same contract as the SupervisorPlacement.hpp functions it + // calls. + std::vector process(const mjModel* m, mjData* d, const gc::GameControllerPacket& pkt) { + std::vector actions; + + const gc::State old_state = prev_ ? prev_->state : gc::UNKNOWN_STATE; + const gc::State new_state = pkt.state; + const bool state_changed = new_state != old_state; + + if (state_changed) { + actions.push_back({Action::Level::INFO, std::string("state -> ") + gc::state_name(new_state)}); + } - if (!prev_ || prev_->kicking_team != pkt.kicking_team) { - // Logged only: which team has kickoff doesn't change where the - // ball goes (always centre), see the task brief this module was - // built against. Kept here because a future robot-positioning - // rule (kicking team must stay outside the centre circle, etc.) - // would need it, and it's what "parse enough to know kickoff - // team" (this module's spec) means in the meantime. - actions.push_back({Action::Level::INFO, "kickoff team id -> " + std::to_string(pkt.kicking_team)}); - } + if (!prev_ || prev_->kicking_team != pkt.kicking_team) { + // Logged only: which team has kickoff doesn't change where the + // ball goes (always centre), see the task brief this module was + // built against. Kept here because a future robot-positioning + // rule (kicking team must stay outside the centre circle, etc.) + // would need it, and it's what "parse enough to know kickoff + // team" (this module's spec) means in the meantime. + actions.push_back({Action::Level::INFO, "kickoff team id -> " + std::to_string(pkt.kicking_team)}); + } - // Core kickoff rule: ball -> centre the instant we enter READY - // (teams are now walking to kickoff positions with a known ball spot - // — this is also what real matches/Webots show) or PLAYING (SET-> - // PLAYING is the literal kickoff whistle). Using "entered PLAYING" - // rather than requiring specifically SET->PLAYING also covers the - // bootstrap case where the first packet this process ever observes - // already reports PLAYING (old_state is the UNKNOWN_STATE sentinel, - // which never equals PLAYING, so this still fires). - bool center_ball = state_changed && (new_state == gc::State::READY || new_state == gc::State::PLAYING); - - // Optional extra resets (task brief: "On FINISHED/goal/half - // (optional): reset ball to centre"). - if (cfg_.reset_ball_on_finished && state_changed && new_state == gc::State::FINISHED) { - center_ball = true; - } - if (prev_) { - if (cfg_.reset_ball_on_goal && score_increased(*prev_, pkt)) { + // Core kickoff rule: ball -> centre the instant we enter READY + // (teams are now walking to kickoff positions with a known ball spot + // — this is also what real matches/Webots show) or PLAYING (SET-> + // PLAYING is the literal kickoff whistle). Using "entered PLAYING" + // rather than requiring specifically SET->PLAYING also covers the + // bootstrap case where the first packet this process ever observes + // already reports PLAYING (old_state is the UNKNOWN_STATE sentinel, + // which never equals PLAYING, so this still fires). + bool center_ball = state_changed && (new_state == gc::State::READY || new_state == gc::State::PLAYING); + + // Optional extra resets (task brief: "On FINISHED/goal/half + // (optional): reset ball to centre"). + if (cfg_.reset_ball_on_finished && state_changed && new_state == gc::State::FINISHED) { center_ball = true; - actions.push_back({Action::Level::INFO, "goal detected"}); } - if (cfg_.reset_ball_on_half_change && prev_->first_half != pkt.first_half) { - center_ball = true; - actions.push_back({Action::Level::INFO, "half changed"}); + if (prev_) { + if (cfg_.reset_ball_on_goal && score_increased(*prev_, pkt)) { + center_ball = true; + actions.push_back({Action::Level::INFO, "goal detected"}); + } + if (cfg_.reset_ball_on_half_change && prev_->first_half != pkt.first_half) { + center_ball = true; + actions.push_back({Action::Level::INFO, "half changed"}); + } } - } - if (center_ball && cfg_.ball.enabled) { - place_ball(m, d, actions); - } + if (center_ball && cfg_.ball.enabled) { + place_ball(m, d, actions); + } - for (const auto& rc : cfg_.robots) { - apply_robot(m, d, rc, pkt, actions); - } + for (const auto& rc : cfg_.robots) { + apply_robot(m, d, rc, pkt, actions); + } - prev_ = pkt; - return actions; - } + prev_ = pkt; + return actions; + } - // Whether process() has seen a packet yet (idle vs live) — purely - // informational, e.g. for a Startup log line. - [[nodiscard]] bool has_seen_packet() const { - return prev_.has_value(); - } + // Whether process() has seen a packet yet (idle vs live) — purely + // informational, e.g. for a Startup log line. + [[nodiscard]] bool has_seen_packet() const { + return prev_.has_value(); + } -private: - static bool score_increased(const gc::GameControllerPacket& old_pkt, const gc::GameControllerPacket& new_pkt) { - for (std::size_t i = 0; i < old_pkt.teams.size(); ++i) { - if (new_pkt.teams[i].score > old_pkt.teams[i].score) { - return true; + private: + static bool score_increased(const gc::GameControllerPacket& old_pkt, const gc::GameControllerPacket& new_pkt) { + for (std::size_t i = 0; i < old_pkt.teams.size(); ++i) { + if (new_pkt.teams[i].score > old_pkt.teams[i].score) { + return true; + } } + return false; } - return false; - } - - static const gc::Team* resolve_team(const gc::GameControllerPacket& pkt, const RobotConfig& rc) { - if (rc.team_id >= 0) { - for (const auto& t : pkt.teams) { - if (t.team_id == static_cast(rc.team_id)) { - return &t; + + static const gc::Team* resolve_team(const gc::GameControllerPacket& pkt, const RobotConfig& rc) { + if (rc.team_id >= 0) { + for (const auto& t : pkt.teams) { + if (t.team_id == static_cast(rc.team_id)) { + return &t; + } } + return nullptr; + } + if (rc.team_index == 0 || rc.team_index == 1) { + return &pkt.teams[static_cast(rc.team_index)]; } return nullptr; } - if (rc.team_index == 0 || rc.team_index == 1) { - return &pkt.teams[static_cast(rc.team_index)]; - } - return nullptr; - } - void warn_once(std::vector& actions, const std::string& msg) { - if (warned_.insert(msg).second) { - actions.push_back({Action::Level::WARN, msg}); - } - } - - void place_ball(const mjModel* m, mjData* d, std::vector& actions) { - const int body_id = mj_name2id(m, mjOBJ_BODY, cfg_.ball.body.c_str()); - const int geom_id = mj_name2id(m, mjOBJ_GEOM, cfg_.ball.geom.c_str()); - if (body_id < 0 || geom_id < 0) { - warn_once(actions, "ball body '" + cfg_.ball.body + "' or geom '" + cfg_.ball.geom + "' not in model"); - return; + void warn_once(std::vector& actions, const std::string& msg) { + if (warned_.insert(msg).second) { + actions.push_back({Action::Level::WARN, msg}); + } } - double z = cfg_.ball.z; - if (z < 0.0) { - z = m->geom_size[3 * geom_id + 0]; // sphere radius -> rests exactly on the floor plane (z=0) - } + void place_ball(const mjModel* m, mjData* d, std::vector& actions) { + const int body_id = mj_name2id(m, mjOBJ_BODY, cfg_.ball.body.c_str()); + const int geom_id = mj_name2id(m, mjOBJ_GEOM, cfg_.ball.geom.c_str()); + if (body_id < 0 || geom_id < 0) { + warn_once(actions, "ball body '" + cfg_.ball.body + "' or geom '" + cfg_.ball.geom + "' not in model"); + return; + } - if (place_free_body_by_geom_center(m, d, body_id, geom_id, cfg_.ball.x, cfg_.ball.y, z)) { - char buf[128]; - std::snprintf(buf, sizeof(buf), "ball -> centre (%.3f, %.3f, %.3f)", cfg_.ball.x, cfg_.ball.y, z); - actions.push_back({Action::Level::INFO, buf}); - } - else { - warn_once(actions, "ball placement failed (body '" + cfg_.ball.body + "' not free-jointed?)"); - } - } - - void apply_robot(const mjModel* m, - mjData* d, - const RobotConfig& rc, - const gc::GameControllerPacket& pkt, - std::vector& actions) { - const gc::Team* new_team = resolve_team(pkt, rc); - if (new_team == nullptr || rc.player_id < 1 - || static_cast(rc.player_id) > gc::MAX_NUM_PLAYERS) { - return; - } - const gc::PenaltyState new_ps = new_team->players[static_cast(rc.player_id) - 1].penalty_state; + double z = cfg_.ball.z; + if (z < 0.0) { + z = m->geom_size[3 * geom_id + 0]; // sphere radius -> rests exactly on the floor plane (z=0) + } - gc::PenaltyState old_ps = gc::PenaltyState::UNPENALISED; - if (prev_) { - const gc::Team* old_team = resolve_team(*prev_, rc); - if (old_team != nullptr) { - old_ps = old_team->players[static_cast(rc.player_id) - 1].penalty_state; + if (place_free_body_by_geom_center(m, d, body_id, geom_id, cfg_.ball.x, cfg_.ball.y, z)) { + char buf[128]; + std::snprintf(buf, sizeof(buf), "ball -> centre (%.3f, %.3f, %.3f)", cfg_.ball.x, cfg_.ball.y, z); + actions.push_back({Action::Level::INFO, buf}); + } + else { + warn_once(actions, "ball placement failed (body '" + cfg_.ball.body + "' not free-jointed?)"); } } - if (new_ps == old_ps) { - return; - } + void apply_robot(const mjModel* m, + mjData* d, + const RobotConfig& rc, + const gc::GameControllerPacket& pkt, + std::vector& actions) { + const gc::Team* new_team = resolve_team(pkt, rc); + if (new_team == nullptr || rc.player_id < 1 + || static_cast(rc.player_id) > gc::MAX_NUM_PLAYERS) { + return; + } + const gc::PenaltyState new_ps = new_team->players[static_cast(rc.player_id) - 1].penalty_state; - const int body_id = mj_name2id(m, mjOBJ_BODY, rc.body.c_str()); - if (body_id < 0) { - warn_once(actions, "robot body '" + rc.body + "' not in model"); - return; - } + gc::PenaltyState old_ps = gc::PenaltyState::UNPENALISED; + if (prev_) { + const gc::Team* old_team = resolve_team(*prev_, rc); + if (old_team != nullptr) { + old_ps = old_team->players[static_cast(rc.player_id) - 1].penalty_state; + } + } - if (new_ps != gc::PenaltyState::UNPENALISED) { - place_free_body(m, d, body_id, rc.penalty_pose.x, rc.penalty_pose.y, rc.penalty_pose.z, rc.penalty_pose.yaw); - actions.push_back( - {Action::Level::INFO, rc.body + " penalised (" + gc::penalty_state_name(new_ps) + ") -> side line"}); - } - else { - place_free_body(m, d, body_id, rc.home_pose.x, rc.home_pose.y, rc.home_pose.z, rc.home_pose.yaw); - actions.push_back({Action::Level::INFO, rc.body + " unpenalised -> own half"}); + if (new_ps == old_ps) { + return; + } + + const int body_id = mj_name2id(m, mjOBJ_BODY, rc.body.c_str()); + if (body_id < 0) { + warn_once(actions, "robot body '" + rc.body + "' not in model"); + return; + } + + if (new_ps != gc::PenaltyState::UNPENALISED) { + place_free_body(m, + d, + body_id, + rc.penalty_pose.x, + rc.penalty_pose.y, + rc.penalty_pose.z, + rc.penalty_pose.yaw); + actions.push_back({Action::Level::INFO, + rc.body + " penalised (" + gc::penalty_state_name(new_ps) + ") -> side line"}); + } + else { + place_free_body(m, d, body_id, rc.home_pose.x, rc.home_pose.y, rc.home_pose.z, rc.home_pose.yaw); + actions.push_back({Action::Level::INFO, rc.body + " unpenalised -> own half"}); + } } - } - SupervisorConfig cfg_; - std::optional prev_; - std::set warned_; -}; + SupervisorConfig cfg_; + std::optional prev_; + std::set warned_; + }; } // namespace k1sim::module::supervisor diff --git a/mujoco/module/Supervisor/src/SupervisorPlacement.hpp b/mujoco/module/Supervisor/src/SupervisorPlacement.hpp index 24c2ddf..aacfa87 100644 --- a/mujoco/module/Supervisor/src/SupervisorPlacement.hpp +++ b/mujoco/module/Supervisor/src/SupervisorPlacement.hpp @@ -17,69 +17,76 @@ // with mj_makeData and call these with no NUClear/threading involved at all. namespace k1sim::module::supervisor { -// yaw-only orientation (about world +Z) as a MuJoCo wxyz quaternion — all the -// placement poses this module deals with (kickoff spawn, penalty spot) are -// flat-ground standing poses, so pitch/roll are always zero. -inline std::array yaw_to_quat(double yaw_rad) { - const double half = 0.5 * yaw_rad; - return {std::cos(half), 0.0, 0.0, std::sin(half)}; -} - -// Writes body_id's free-joint qpos to the given world position + yaw and -// zeros its qvel (both linear and angular), so the body is dropped in at -// rest with no inherited velocity. body_id must own exactly one joint, and it -// must be a free joint (mjJNT_FREE) — true for the ball body and the robot's -// "Trunk" body in k1_scene_robocup.xml, per that file and K1_22dof.xml. -// Returns false (no write) if body_id is invalid or isn't free-jointed, so -// callers can log a config error instead of corrupting unrelated qpos. -inline bool place_free_body(const mjModel* m, mjData* d, int body_id, double x, double y, double z, double yaw_rad) { - if (m == nullptr || d == nullptr || body_id < 0 || body_id >= m->nbody) { - return false; - } - if (m->body_jntnum[body_id] != 1) { - return false; - } - const int jnt = m->body_jntadr[body_id]; - if (m->jnt_type[jnt] != mjJNT_FREE) { - return false; + // yaw-only orientation (about world +Z) as a MuJoCo wxyz quaternion — all the + // placement poses this module deals with (kickoff spawn, penalty spot) are + // flat-ground standing poses, so pitch/roll are always zero. + inline std::array yaw_to_quat(double yaw_rad) { + const double half = 0.5 * yaw_rad; + return {std::cos(half), 0.0, 0.0, std::sin(half)}; } - const int qpos_adr = m->jnt_qposadr[jnt]; - const int dof_adr = m->jnt_dofadr[jnt]; - const auto q = yaw_to_quat(yaw_rad); + // Writes body_id's free-joint qpos to the given world position + yaw and + // zeros its qvel (both linear and angular), so the body is dropped in at + // rest with no inherited velocity. body_id must own exactly one joint, and it + // must be a free joint (mjJNT_FREE) — true for the ball body and the robot's + // "Trunk" body in k1_scene_robocup.xml, per that file and K1_22dof.xml. + // Returns false (no write) if body_id is invalid or isn't free-jointed, so + // callers can log a config error instead of corrupting unrelated qpos. + inline bool + place_free_body(const mjModel* m, mjData* d, int body_id, double x, double y, double z, double yaw_rad) { + if (m == nullptr || d == nullptr || body_id < 0 || body_id >= m->nbody) { + return false; + } + if (m->body_jntnum[body_id] != 1) { + return false; + } + const int jnt = m->body_jntadr[body_id]; + if (m->jnt_type[jnt] != mjJNT_FREE) { + return false; + } + + const int qpos_adr = m->jnt_qposadr[jnt]; + const int dof_adr = m->jnt_dofadr[jnt]; + const auto q = yaw_to_quat(yaw_rad); - d->qpos[qpos_adr + 0] = x; - d->qpos[qpos_adr + 1] = y; - d->qpos[qpos_adr + 2] = z; - d->qpos[qpos_adr + 3] = q[0]; - d->qpos[qpos_adr + 4] = q[1]; - d->qpos[qpos_adr + 5] = q[2]; - d->qpos[qpos_adr + 6] = q[3]; - for (int i = 0; i < 6; ++i) { - d->qvel[dof_adr + i] = 0.0; + d->qpos[qpos_adr + 0] = x; + d->qpos[qpos_adr + 1] = y; + d->qpos[qpos_adr + 2] = z; + d->qpos[qpos_adr + 3] = q[0]; + d->qpos[qpos_adr + 4] = q[1]; + d->qpos[qpos_adr + 5] = q[2]; + d->qpos[qpos_adr + 6] = q[3]; + for (int i = 0; i < 6; ++i) { + d->qvel[dof_adr + i] = 0.0; + } + return true; } - return true; -} -// Like place_free_body, but the target (x,y,z) is the world-space centre of -// one of the body's own geoms rather than the body origin itself — needed for -// k1_scene_robocup.xml's "ball" body, whose sphere geom is deliberately kept -// off the body origin (a freejoint-keyframe zero-padding workaround; see that -// file's header comment). Solves target = qpos_xyz + geom_local_pos for -// qpos_xyz (valid because the ball geom has no , i.e. zero -// rotation relative to its body, and this function always places the body at -// identity orientation — a rotated offset would need the body's orientation -// folded in too, which no current use case needs). For a geom that already -// sits at its body's origin this is exactly place_free_body(..., yaw=0). -inline bool place_free_body_by_geom_center(const mjModel* m, mjData* d, int body_id, int geom_id, double x, double y, double z) { - if (m == nullptr || geom_id < 0 || geom_id >= m->ngeom || m->geom_bodyid[geom_id] != body_id) { - return false; + // Like place_free_body, but the target (x,y,z) is the world-space centre of + // one of the body's own geoms rather than the body origin itself — needed for + // k1_scene_robocup.xml's "ball" body, whose sphere geom is deliberately kept + // off the body origin (a freejoint-keyframe zero-padding workaround; see that + // file's header comment). Solves target = qpos_xyz + geom_local_pos for + // qpos_xyz (valid because the ball geom has no , i.e. zero + // rotation relative to its body, and this function always places the body at + // identity orientation — a rotated offset would need the body's orientation + // folded in too, which no current use case needs). For a geom that already + // sits at its body's origin this is exactly place_free_body(..., yaw=0). + inline bool place_free_body_by_geom_center(const mjModel* m, + mjData* d, + int body_id, + int geom_id, + double x, + double y, + double z) { + if (m == nullptr || geom_id < 0 || geom_id >= m->ngeom || m->geom_bodyid[geom_id] != body_id) { + return false; + } + const double ox = m->geom_pos[3 * geom_id + 0]; + const double oy = m->geom_pos[3 * geom_id + 1]; + const double oz = m->geom_pos[3 * geom_id + 2]; + return place_free_body(m, d, body_id, x - ox, y - oy, z - oz, 0.0); } - const double ox = m->geom_pos[3 * geom_id + 0]; - const double oy = m->geom_pos[3 * geom_id + 1]; - const double oz = m->geom_pos[3 * geom_id + 2]; - return place_free_body(m, d, body_id, x - ox, y - oy, z - oz, 0.0); -} } // namespace k1sim::module::supervisor diff --git a/mujoco/module/Viewer/CMakeLists.txt b/mujoco/module/Viewer/CMakeLists.txt index 123bfe4..f1db8ac 100644 --- a/mujoco/module/Viewer/CMakeLists.txt +++ b/mujoco/module/Viewer/CMakeLists.txt @@ -2,7 +2,7 @@ find_package(OpenGL REQUIRED) add_library(k1sim_module_viewer STATIC src/Viewer.cpp) target_link_libraries( - k1sim_module_viewer - PUBLIC k1sim_shared - PRIVATE glfw OpenGL::GL + k1sim_module_viewer + PUBLIC k1sim_shared + PRIVATE glfw OpenGL::GL ) diff --git a/mujoco/module/Viewer/src/Viewer.cpp b/mujoco/module/Viewer/src/Viewer.cpp index 673a3a2..e76ec07 100644 --- a/mujoco/module/Viewer/src/Viewer.cpp +++ b/mujoco/module/Viewer/src/Viewer.cpp @@ -1,11 +1,10 @@ #include "module/Viewer/src/Viewer.hpp" #include -#include - #include #include #include +#include #include #include "shared/CliOptions.hpp" @@ -15,358 +14,362 @@ namespace k1sim::module { -namespace { - -// GLFW callbacks must be plain C function pointers, and this Reactor is only -// ever installed once (PowerPlant::install() is called exactly once -// from main.cpp), so the GL/mjv/mjr state lives here as translation-unit -// globals rather than as Viewer members — that keeps GLFW/MuJoCo headers out -// of Viewer.hpp (main.cpp only needs the Reactor type, matching every other -// module in this tree). - -GLFWwindow* window = nullptr; - -mjvCamera cam; -mjvOption opt; -mjvPerturb pert; -mjvScene scn; -mjrContext con; -bool scene_ready = false; - -// Cached from SimHandles (Trigger, set once after the -// model loads). model/data/mutex are owned by module::Simulation. -const mjModel* g_model = nullptr; -mjData* g_data = nullptr; -std::mutex* g_mutex = nullptr; -std::atomic* g_measured_rtf = nullptr; - -// Cached from SimStateUpdate (50 Hz) — overlay text only, never used for -// anything physics-critical, so plain atomics without the sim mutex are fine. -std::atomic g_sim_time{0.0}; -std::atomic g_mode{-1}; - -// Set by the Viewer constructor; lets the plain-C GLFW key callback emit a -// NUClear message (SimResetRequest) without holding a Reactor pointer here. -std::function g_request_reset; - -// Mouse state for the simulate-style camera + perturb controls. -bool button_left = false; -bool button_middle = false; -bool button_right = false; -double last_x = 0.0; -double last_y = 0.0; - -const char* mode_name(int mode) { - switch (mode) { - case booster::DAMPING: return "DAMPING"; - case booster::PREPARE: return "PREPARE"; - case booster::WALKING: return "WALKING"; - case booster::CUSTOM: return "CUSTOM"; - case booster::SOCCER: return "SOCCER"; - default: return "?"; - } -} + namespace { + + // GLFW callbacks must be plain C function pointers, and this Reactor is only + // ever installed once (PowerPlant::install() is called exactly once + // from main.cpp), so the GL/mjv/mjr state lives here as translation-unit + // globals rather than as Viewer members — that keeps GLFW/MuJoCo headers out + // of Viewer.hpp (main.cpp only needs the Reactor type, matching every other + // module in this tree). + + GLFWwindow* window = nullptr; + + mjvCamera cam; + mjvOption opt; + mjvPerturb pert; + mjvScene scn; + mjrContext con; + bool scene_ready = false; + + // Cached from SimHandles (Trigger, set once after the + // model loads). model/data/mutex are owned by module::Simulation. + const mjModel* g_model = nullptr; + mjData* g_data = nullptr; + std::mutex* g_mutex = nullptr; + std::atomic* g_measured_rtf = nullptr; + + // Cached from SimStateUpdate (50 Hz) — overlay text only, never used for + // anything physics-critical, so plain atomics without the sim mutex are fine. + std::atomic g_sim_time{0.0}; + std::atomic g_mode{-1}; + + // Set by the Viewer constructor; lets the plain-C GLFW key callback emit a + // NUClear message (SimResetRequest) without holding a Reactor pointer here. + std::function g_request_reset; + + // Mouse state for the simulate-style camera + perturb controls. + bool button_left = false; + bool button_middle = false; + bool button_right = false; + double last_x = 0.0; + double last_y = 0.0; + + const char* mode_name(int mode) { + switch (mode) { + case booster::DAMPING: return "DAMPING"; + case booster::PREPARE: return "PREPARE"; + case booster::WALKING: return "WALKING"; + case booster::CUSTOM: return "CUSTOM"; + case booster::SOCCER: return "SOCCER"; + default: return "?"; + } + } -// Release any perturbation force/selection so a stray drag never leaves the -// robot pinned once the mouse button comes back up. -void end_perturb() { - pert.active = 0; -} + // Release any perturbation force/selection so a stray drag never leaves the + // robot pinned once the mouse button comes back up. + void end_perturb() { + pert.active = 0; + } -void keyboard_cb(GLFWwindow* w, int key, int /*scancode*/, int act, int /*mods*/) { - if (act != GLFW_PRESS) { - return; - } - if (key == GLFW_KEY_ESCAPE) { - glfwSetWindowShouldClose(w, GLFW_TRUE); - } - // BACKSPACE (MuJoCo simulate convention): reset the sim to its startup state. - if (key == GLFW_KEY_BACKSPACE && g_request_reset) { - end_perturb(); // a reset mid-drag must not leave a perturb force pinned - g_request_reset(); - } - // F: shove the robot over (deterministic fall for testing FallRecovery/GetUp — - // mouse-drag perturbs are usually within what the push-randomised policy rides out). - if (key == GLFW_KEY_F && g_model != nullptr && g_data != nullptr && g_mutex != nullptr) { - std::lock_guard lock(*g_mutex); - if (g_model->njnt > 0 && g_model->jnt_type[0] == mjJNT_FREE) { - const int dof = g_model->jnt_dofadr[0]; - g_data->qvel[dof + 0] += 1.5; // linear kick, world x - g_data->qvel[dof + 4] += 6.0; // pitch rate — guarantees a topple + void keyboard_cb(GLFWwindow* w, int key, int /*scancode*/, int act, int /*mods*/) { + if (act != GLFW_PRESS) { + return; + } + if (key == GLFW_KEY_ESCAPE) { + glfwSetWindowShouldClose(w, GLFW_TRUE); + } + // BACKSPACE (MuJoCo simulate convention): reset the sim to its startup state. + if (key == GLFW_KEY_BACKSPACE && g_request_reset) { + end_perturb(); // a reset mid-drag must not leave a perturb force pinned + g_request_reset(); + } + // F: shove the robot over (deterministic fall for testing FallRecovery/GetUp — + // mouse-drag perturbs are usually within what the push-randomised policy rides out). + if (key == GLFW_KEY_F && g_model != nullptr && g_data != nullptr && g_mutex != nullptr) { + std::lock_guard lock(*g_mutex); + if (g_model->njnt > 0 && g_model->jnt_type[0] == mjJNT_FREE) { + const int dof = g_model->jnt_dofadr[0]; + g_data->qvel[dof + 0] += 1.5; // linear kick, world x + g_data->qvel[dof + 4] += 6.0; // pitch rate — guarantees a topple + } + } + // SPACE (pause) is intentionally NOT wired here: pausing physics is + // module::Simulation's domain (it owns the 1 kHz stepping thread), and + // there's no pause switch exposed yet. Future work once Simulation grows + // one — see docs/K1_MUJOCO_SETUP.md known limitations. } - } - // SPACE (pause) is intentionally NOT wired here: pausing physics is - // module::Simulation's domain (it owns the 1 kHz stepping thread), and - // there's no pause switch exposed yet. Future work once Simulation grows - // one — see docs/K1_MUJOCO_SETUP.md known limitations. -} - -void mouse_button_cb(GLFWwindow* w, int button, int act, int mods) { - button_left = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS; - button_middle = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS; - button_right = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS; - glfwGetCursorPos(w, &last_x, &last_y); - - if (g_model == nullptr || g_data == nullptr || g_mutex == nullptr) { - return; - } - if (act == GLFW_RELEASE) { - end_perturb(); - return; - } + void mouse_button_cb(GLFWwindow* w, int button, int act, int mods) { + button_left = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS; + button_middle = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS; + button_right = glfwGetMouseButton(w, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS; + glfwGetCursorPos(w, &last_x, &last_y); - const bool mod_ctrl = (mods & GLFW_MOD_CONTROL) != 0; + if (g_model == nullptr || g_data == nullptr || g_mutex == nullptr) { + return; + } - std::lock_guard lock(*g_mutex); + if (act == GLFW_RELEASE) { + end_perturb(); + return; + } - // Ctrl + drag on the already-selected body: push/twist it (get-up/fall testing). - if (mod_ctrl && pert.select > 0) { - if (button_right) { - mjv_initPerturb(g_model, g_data, &scn, &pert); - pert.active = mjPERT_TRANSLATE; - } - else if (button_left) { - mjv_initPerturb(g_model, g_data, &scn, &pert); - pert.active = mjPERT_ROTATE; - } - } + const bool mod_ctrl = (mods & GLFW_MOD_CONTROL) != 0; - // Double-click (any button, <300 ms apart): select the body under the cursor. - static double last_click_time = 0.0; - static int last_click_button = -1; - const double now = glfwGetTime(); - const bool double_click = (button == last_click_button) && (now - last_click_time < 0.3); - last_click_time = now; - last_click_button = button; - - if (double_click) { - int width = 0; - int height = 0; - glfwGetWindowSize(w, &width, &height); - if (width > 0 && height > 0) { - const double aspect = static_cast(width) / static_cast(height); - const double relx = last_x / width; - const double rely = 1.0 - last_y / height; - - mjtNum selpnt[3]; - int geomid = -1; - int flexid = -1; - int skinid = -1; - const int body = - mjv_select(g_model, g_data, &opt, aspect, relx, rely, &scn, selpnt, &geomid, &flexid, &skinid); - - if (body >= 0) { - pert.select = body; - pert.skinselect = skinid; - mjtNum tmp[3]; - mju_sub3(tmp, selpnt, g_data->xpos + 3 * body); - mju_mulMatTVec3(pert.localpos, g_data->xmat + 9 * body, tmp); + std::lock_guard lock(*g_mutex); + + // Ctrl + drag on the already-selected body: push/twist it (get-up/fall testing). + if (mod_ctrl && pert.select > 0) { + if (button_right) { + mjv_initPerturb(g_model, g_data, &scn, &pert); + pert.active = mjPERT_TRANSLATE; + } + else if (button_left) { + mjv_initPerturb(g_model, g_data, &scn, &pert); + pert.active = mjPERT_ROTATE; + } } - else { - pert.select = 0; - pert.skinselect = -1; - end_perturb(); + + // Double-click (any button, <300 ms apart): select the body under the cursor. + static double last_click_time = 0.0; + static int last_click_button = -1; + const double now = glfwGetTime(); + const bool double_click = (button == last_click_button) && (now - last_click_time < 0.3); + last_click_time = now; + last_click_button = button; + + if (double_click) { + int width = 0; + int height = 0; + glfwGetWindowSize(w, &width, &height); + if (width > 0 && height > 0) { + const double aspect = static_cast(width) / static_cast(height); + const double relx = last_x / width; + const double rely = 1.0 - last_y / height; + + mjtNum selpnt[3]; + int geomid = -1; + int flexid = -1; + int skinid = -1; + const int body = + mjv_select(g_model, g_data, &opt, aspect, relx, rely, &scn, selpnt, &geomid, &flexid, &skinid); + + if (body >= 0) { + pert.select = body; + pert.skinselect = skinid; + mjtNum tmp[3]; + mju_sub3(tmp, selpnt, g_data->xpos + 3 * body); + mju_mulMatTVec3(pert.localpos, g_data->xmat + 9 * body, tmp); + } + else { + pert.select = 0; + pert.skinselect = -1; + end_perturb(); + } + } } } - } -} -void mouse_move_cb(GLFWwindow* w, double xpos, double ypos) { - const double dx = xpos - last_x; - const double dy = ypos - last_y; - last_x = xpos; - last_y = ypos; + void mouse_move_cb(GLFWwindow* w, double xpos, double ypos) { + const double dx = xpos - last_x; + const double dy = ypos - last_y; + last_x = xpos; + last_y = ypos; - if (!button_left && !button_middle && !button_right) { - return; - } - if (g_model == nullptr) { - return; - } - - int width = 0; - int height = 0; - glfwGetWindowSize(w, &width, &height); - if (height == 0) { - return; - } + if (!button_left && !button_middle && !button_right) { + return; + } + if (g_model == nullptr) { + return; + } - const bool mod_shift = glfwGetKey(w, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS - || glfwGetKey(w, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS; - const bool mod_ctrl = glfwGetKey(w, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS - || glfwGetKey(w, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS; + int width = 0; + int height = 0; + glfwGetWindowSize(w, &width, &height); + if (height == 0) { + return; + } - if (mod_ctrl && pert.active != 0 && g_data != nullptr && g_mutex != nullptr) { - const mjtMouse action = button_right ? mjMOUSE_MOVE_V : mjMOUSE_ROTATE_V; - std::lock_guard lock(*g_mutex); - mjv_movePerturb(g_model, g_data, action, dx / height, dy / height, &scn, &pert); - return; - } + const bool mod_shift = + glfwGetKey(w, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS || glfwGetKey(w, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS; + const bool mod_ctrl = glfwGetKey(w, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS + || glfwGetKey(w, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS; - mjtMouse action = mjMOUSE_ZOOM; - if (button_right) { - action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; - } - else if (button_left) { - action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; - } - mjv_moveCamera(g_model, action, dx / height, dy / height, &scn, &cam); -} + if (mod_ctrl && pert.active != 0 && g_data != nullptr && g_mutex != nullptr) { + const mjtMouse action = button_right ? mjMOUSE_MOVE_V : mjMOUSE_ROTATE_V; + std::lock_guard lock(*g_mutex); + mjv_movePerturb(g_model, g_data, action, dx / height, dy / height, &scn, &pert); + return; + } -void scroll_cb(GLFWwindow* /*w*/, double /*xoffset*/, double yoffset) { - if (g_model == nullptr) { - return; - } - mjv_moveCamera(g_model, mjMOUSE_ZOOM, 0.0, -0.05 * yoffset, &scn, &cam); -} + mjtMouse action = mjMOUSE_ZOOM; + if (button_right) { + action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; + } + else if (button_left) { + action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; + } + mjv_moveCamera(g_model, action, dx / height, dy / height, &scn, &cam); + } -} // namespace + void scroll_cb(GLFWwindow* /*w*/, double /*xoffset*/, double yoffset) { + if (g_model == nullptr) { + return; + } + mjv_moveCamera(g_model, mjMOUSE_ZOOM, 0.0, -0.05 * yoffset, &scn, &cam); + } -Viewer::Viewer(std::unique_ptr environment) : Reactor(std::move(environment)) { + } // namespace - // Bridge for the plain-C GLFW key callback (Backspace = sim reset). emit() is - // thread-safe, and the callback only ever runs on the MainThread glfwPollEvents pump. - g_request_reset = [this] { emit(std::make_unique()); }; + Viewer::Viewer(std::unique_ptr environment) : Reactor(std::move(environment)) { - on().then([this] { - if (cli().headless) { - log("Viewer disabled (--headless)"); - return; - } + // Bridge for the plain-C GLFW key callback (Backspace = sim reset). emit() is + // thread-safe, and the callback only ever runs on the MainThread glfwPollEvents pump. + g_request_reset = [this] { emit(std::make_unique()); }; - if (glfwInit() == GLFW_FALSE) { - log("glfwInit() failed — continuing without a viewer window"); - return; - } + on().then([this] { + if (cli().headless) { + log("Viewer disabled (--headless)"); + return; + } - window = glfwCreateWindow(1200, 900, "K1 MuJoCo Sim", nullptr, nullptr); - if (window == nullptr) { - log("glfwCreateWindow() failed — continuing without a viewer window"); - glfwTerminate(); - return; - } + if (glfwInit() == GLFW_FALSE) { + log("glfwInit() failed — continuing without a viewer window"); + return; + } - glfwMakeContextCurrent(window); - // No vsync: frame pacing is already owned by the Every<16ms> reaction - // below, and blocking glfwSwapBuffers() on a vblank the driver never - // delivers (headless/virtual displays, some remote X servers) would - // stall the NUClear MainThread pool — the same thread glfwPollEvents - // needs, so the whole viewer (and its window-close handling) hangs. - glfwSwapInterval(0); - - mjv_defaultCamera(&cam); - mjv_defaultOption(&opt); - mjv_defaultPerturb(&pert); - mjr_defaultContext(&con); - // mjv_makeScene/mjr_makeContext need an mjModel, so they're deferred - // to the Trigger reaction below (also MainThread — GL - // context creation is thread-bound to whichever thread called - // glfwMakeContextCurrent, i.e. this one). - - glfwSetMouseButtonCallback(window, mouse_button_cb); - glfwSetCursorPosCallback(window, mouse_move_cb); - glfwSetScrollCallback(window, scroll_cb); - glfwSetKeyCallback(window, keyboard_cb); - - log("Viewer window created (1200x900) — waiting for the simulation model"); - }); - - on, MainThread>().then([this](const message::SimHandles& handles) { - if (cli().headless) { - return; - } + window = glfwCreateWindow(1200, 900, "K1 MuJoCo Sim", nullptr, nullptr); + if (window == nullptr) { + log("glfwCreateWindow() failed — continuing without a viewer window"); + glfwTerminate(); + return; + } - g_model = handles.model; - g_data = handles.data; - g_mutex = handles.mutex; - g_measured_rtf = handles.measured_rtf; - - if (window != nullptr && !scene_ready && g_model != nullptr) { - mjv_makeScene(g_model, &scn, 2000); - mjr_makeContext(g_model, &con, mjFONTSCALE_150); - // mjv_defaultCamera (Startup) doesn't know the model's scale; now - // that it's loaded, frame the free camera on it (lookat/distance - // from the compiled model extent) so the scene isn't clipped or - // too distant to see on first frame. - mjv_defaultFreeCamera(g_model, &cam); - scene_ready = true; - log("Viewer scene ready (", g_model->nbody, "bodies,", g_model->ngeom, "geoms)"); - } - }); + glfwMakeContextCurrent(window); + // No vsync: frame pacing is already owned by the Every<16ms> reaction + // below, and blocking glfwSwapBuffers() on a vblank the driver never + // delivers (headless/virtual displays, some remote X servers) would + // stall the NUClear MainThread pool — the same thread glfwPollEvents + // needs, so the whole viewer (and its window-close handling) hangs. + glfwSwapInterval(0); + + mjv_defaultCamera(&cam); + mjv_defaultOption(&opt); + mjv_defaultPerturb(&pert); + mjr_defaultContext(&con); + // mjv_makeScene/mjr_makeContext need an mjModel, so they're deferred + // to the Trigger reaction below (also MainThread — GL + // context creation is thread-bound to whichever thread called + // glfwMakeContextCurrent, i.e. this one). + + glfwSetMouseButtonCallback(window, mouse_button_cb); + glfwSetCursorPosCallback(window, mouse_move_cb); + glfwSetScrollCallback(window, scroll_cb); + glfwSetKeyCallback(window, keyboard_cb); + + log("Viewer window created (1200x900) — waiting for the simulation model"); + }); + + on, MainThread>().then([this](const message::SimHandles& handles) { + if (cli().headless) { + return; + } - // Overlay-only telemetry — never touches mjData, so no MainThread/mutex needed. - on>().then([](const message::SimStateUpdate& state) { - g_sim_time.store(state.sim_time, std::memory_order_relaxed); - g_mode.store(state.mode, std::memory_order_relaxed); - }); + g_model = handles.model; + g_data = handles.data; + g_mutex = handles.mutex; + g_measured_rtf = handles.measured_rtf; + + if (window != nullptr && !scene_ready && g_model != nullptr) { + mjv_makeScene(g_model, &scn, 2000); + mjr_makeContext(g_model, &con, mjFONTSCALE_150); + // mjv_defaultCamera (Startup) doesn't know the model's scale; now + // that it's loaded, frame the free camera on it (lookat/distance + // from the compiled model extent) so the scene isn't clipped or + // too distant to see on first frame. + mjv_defaultFreeCamera(g_model, &cam); + scene_ready = true; + log("Viewer scene ready (", + g_model->nbody, + "bodies,", + g_model->ngeom, + "geoms)"); + } + }); - on, MainThread>().then([this] { - if (cli().headless || window == nullptr) { - return; - } + // Overlay-only telemetry — never touches mjData, so no MainThread/mutex needed. + on>().then([](const message::SimStateUpdate& state) { + g_sim_time.store(state.sim_time, std::memory_order_relaxed); + g_mode.store(state.mode, std::memory_order_relaxed); + }); - if (glfwWindowShouldClose(window) != 0) { - powerplant.shutdown(); - return; - } + on, MainThread>().then([this] { + if (cli().headless || window == nullptr) { + return; + } - glfwPollEvents(); + if (glfwWindowShouldClose(window) != 0) { + powerplant.shutdown(); + return; + } - if (scene_ready && g_data != nullptr && g_mutex != nullptr) { - // Lock only around the mjData touch points: applying the perturb - // force/clearing it, and mjv_updateScene's read of mjData. Both - // are sub-millisecond; the physics thread never blocks on us for - // longer than that. - std::lock_guard lock(*g_mutex); - if (pert.select > 0) { - if (pert.active != 0) { - mjv_applyPerturbForce(g_model, g_data, &pert); - } - else { - mju_zero(g_data->xfrc_applied + 6 * pert.select, 6); + glfwPollEvents(); + + if (scene_ready && g_data != nullptr && g_mutex != nullptr) { + // Lock only around the mjData touch points: applying the perturb + // force/clearing it, and mjv_updateScene's read of mjData. Both + // are sub-millisecond; the physics thread never blocks on us for + // longer than that. + std::lock_guard lock(*g_mutex); + if (pert.select > 0) { + if (pert.active != 0) { + mjv_applyPerturbForce(g_model, g_data, &pert); + } + else { + mju_zero(g_data->xfrc_applied + 6 * pert.select, 6); + } } + mjv_updateScene(g_model, g_data, &opt, &pert, &cam, mjCAT_ALL, &scn); } - mjv_updateScene(g_model, g_data, &opt, &pert, &cam, mjCAT_ALL, &scn); - } - - int width = 0; - int height = 0; - glfwGetFramebufferSize(window, &width, &height); - const mjrRect viewport{0, 0, width, height}; - - if (scene_ready) { - mjr_render(viewport, &scn, &con); - - const double rtf = (g_measured_rtf != nullptr) ? g_measured_rtf->load(std::memory_order_relaxed) : 0.0; - char overlay[256]; - std::snprintf(overlay, - sizeof(overlay), - "sim time: %.1f s\nRTF: %.2f\nmode: %s", - g_sim_time.load(std::memory_order_relaxed), - rtf, - mode_name(g_mode.load(std::memory_order_relaxed))); - mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, viewport, overlay, nullptr, &con); - } - glfwSwapBuffers(window); - }); + int width = 0; + int height = 0; + glfwGetFramebufferSize(window, &width, &height); + const mjrRect viewport{0, 0, width, height}; - on().then([this] { - if (!cli().headless) { if (scene_ready) { - mjv_freeScene(&scn); - mjr_freeContext(&con); - scene_ready = false; + mjr_render(viewport, &scn, &con); + + const double rtf = (g_measured_rtf != nullptr) ? g_measured_rtf->load(std::memory_order_relaxed) : 0.0; + char overlay[256]; + std::snprintf(overlay, + sizeof(overlay), + "sim time: %.1f s\nRTF: %.2f\nmode: %s", + g_sim_time.load(std::memory_order_relaxed), + rtf, + mode_name(g_mode.load(std::memory_order_relaxed))); + mjr_overlay(mjFONT_NORMAL, mjGRID_TOPLEFT, viewport, overlay, nullptr, &con); } - if (window != nullptr) { - glfwDestroyWindow(window); - window = nullptr; + + glfwSwapBuffers(window); + }); + + on().then([this] { + if (!cli().headless) { + if (scene_ready) { + mjv_freeScene(&scn); + mjr_freeContext(&con); + scene_ready = false; + } + if (window != nullptr) { + glfwDestroyWindow(window); + window = nullptr; + } + glfwTerminate(); } - glfwTerminate(); - } - log("Viewer shutting down"); - }); -} + log("Viewer shutting down"); + }); + } } // namespace k1sim::module diff --git a/mujoco/module/Viewer/src/Viewer.hpp b/mujoco/module/Viewer/src/Viewer.hpp index d081a25..2ba02d7 100644 --- a/mujoco/module/Viewer/src/Viewer.hpp +++ b/mujoco/module/Viewer/src/Viewer.hpp @@ -5,14 +5,14 @@ namespace k1sim::module { -// GLFW window + MuJoCo mjv/mjr GPU rendering, on NUClear's MainThread pool -// (GLFW requires the true main thread; PowerPlant::start() runs MainThread -// reactions there). Skipped entirely under --headless. -// Implementation lands with workstream E (M3); this is the M0 stub. -class Viewer : public NUClear::Reactor { -public: - explicit Viewer(std::unique_ptr environment); -}; + // GLFW window + MuJoCo mjv/mjr GPU rendering, on NUClear's MainThread pool + // (GLFW requires the true main thread; PowerPlant::start() runs MainThread + // reactions there). Skipped entirely under --headless. + // Implementation lands with workstream E (M3); this is the M0 stub. + class Viewer : public NUClear::Reactor { + public: + explicit Viewer(std::unique_ptr environment); + }; } // namespace k1sim::module diff --git a/mujoco/roles/sim/soccer.role b/mujoco/roles/sim/soccer.role index ec0d583..d98fd8d 100644 --- a/mujoco/roles/sim/soccer.role +++ b/mujoco/roles/sim/soccer.role @@ -1,11 +1,11 @@ -# Full soccer simulation: physics + DDS bridge + locomotion + head camera + -# GameController supervisor + viewer. ConsoleLog first so it captures startup logs. +# Full soccer simulation: physics + DDS bridge + locomotion + head camera + GameController supervisor + viewer. +# ConsoleLog first so it captures startup logs. k1sim_role( - ConsoleLog - Simulation - SdkBridge - Locomotion - Camera - Supervisor - Viewer + ConsoleLog + Simulation + SdkBridge + Locomotion + Camera + Supervisor + Viewer ) diff --git a/mujoco/shared/CliOptions.hpp b/mujoco/shared/CliOptions.hpp index 4fa689d..67a0552 100644 --- a/mujoco/shared/CliOptions.hpp +++ b/mujoco/shared/CliOptions.hpp @@ -8,80 +8,81 @@ namespace k1sim { -struct CliOptions { - bool headless = false; - std::string field; // override for simulation.yaml field (a name under its `fields`) - std::string model; // MJCF scene path, overriding the field's scene - std::string config_dir; // override for the config directory - std::string keyframe; // override for the startup keyframe (default "ready") - double rtf = -1.0; // override real-time factor; <0 = use config (0 = free-run) - int robots = 1; // total K1s on the field; extras are PD-held at "ready" -}; + struct CliOptions { + bool headless = false; + std::string field; // override for simulation.yaml field (a name under its `fields`) + std::string model; // MJCF scene path, overriding the field's scene + std::string config_dir; // override for the config directory + std::string keyframe; // override for the startup keyframe (default "ready") + double rtf = -1.0; // override real-time factor; <0 = use config (0 = free-run) + int robots = 1; // total K1s on the field; extras are PD-held at "ready" + }; -inline constexpr int MAX_ROBOTS = 20; + inline constexpr int MAX_ROBOTS = 20; -// Set once in main() before the PowerPlant starts; read-only afterwards. -inline CliOptions& cli() { - static CliOptions options; - return options; -} + // Set once in main() before the PowerPlant starts; read-only afterwards. + inline CliOptions& cli() { + static CliOptions options; + return options; + } -inline CliOptions parse_cli(int argc, char** argv) { - CliOptions opts; - for (int i = 1; i < argc; ++i) { - const std::string arg = argv[i]; - auto value = [&](const char* flag) -> std::string { - if (i + 1 >= argc) { - std::fprintf(stderr, "%s requires a value\n", flag); - std::exit(1); + inline CliOptions parse_cli(int argc, char** argv) { + CliOptions opts; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + auto value = [&](const char* flag) -> std::string { + if (i + 1 >= argc) { + std::fprintf(stderr, "%s requires a value\n", flag); + std::exit(1); + } + return argv[++i]; + }; + if (arg == "--headless") { + opts.headless = true; } - return argv[++i]; - }; - if (arg == "--headless") { - opts.headless = true; - } - else if (arg == "--field") { - opts.field = value("--field"); - } - else if (arg == "--model") { - opts.model = value("--model"); - } - else if (arg == "--config-dir") { - opts.config_dir = value("--config-dir"); - } - else if (arg == "--keyframe") { - opts.keyframe = value("--keyframe"); - } - else if (arg == "--rtf") { - opts.rtf = std::stod(value("--rtf")); - } - else if (arg == "--robots") { - opts.robots = std::stoi(value("--robots")); - if (opts.robots < 1 || opts.robots > MAX_ROBOTS) { - std::fprintf(stderr, "--robots must be between 1 and %d\n", MAX_ROBOTS); + else if (arg == "--field") { + opts.field = value("--field"); + } + else if (arg == "--model") { + opts.model = value("--model"); + } + else if (arg == "--config-dir") { + opts.config_dir = value("--config-dir"); + } + else if (arg == "--keyframe") { + opts.keyframe = value("--keyframe"); + } + else if (arg == "--rtf") { + opts.rtf = std::stod(value("--rtf")); + } + else if (arg == "--robots") { + opts.robots = std::stoi(value("--robots")); + if (opts.robots < 1 || opts.robots > MAX_ROBOTS) { + std::fprintf(stderr, "--robots must be between 1 and %d\n", MAX_ROBOTS); + std::exit(1); + } + } + else if (arg == "--help" || arg == "-h") { + std::printf( + "k1_mujoco_sim — MuJoCo simulator for the Booster K1 (Booster SDK DDS surface)\n" + " --headless run without the viewer window\n" + " --field field to play on, from simulation.yaml's fields:\n" + " middle (RoboCup 2026 M-Field, default) or kidsize\n" + " --model MJCF scene to load, overriding --field\n" + " --config-dir config directory (default: mujoco/config)\n" + " --keyframe startup keyframe (default: ready; e.g. lying_front)\n" + " --rtf real-time factor; 0 = free-run\n" + " --robots total K1s on the field, 1-20 (default 1); extra robots\n" + " are uncontrolled and PD-held at the ready pose\n"); + std::exit(0); + } + else { + std::fprintf(stderr, "unknown argument '%s' (see --help)\n", arg.c_str()); std::exit(1); } } - else if (arg == "--help" || arg == "-h") { - std::printf("k1_mujoco_sim — MuJoCo simulator for the Booster K1 (Booster SDK DDS surface)\n" - " --headless run without the viewer window\n" - " --field field to play on, from simulation.yaml's fields:\n" - " middle (RoboCup 2026 M-Field, default) or kidsize\n" - " --model MJCF scene to load, overriding --field\n" - " --config-dir config directory (default: mujoco/config)\n" - " --keyframe startup keyframe (default: ready; e.g. lying_front)\n" - " --rtf real-time factor; 0 = free-run\n" - " --robots total K1s on the field, 1-20 (default 1); extra robots\n" - " are uncontrolled and PD-held at the ready pose\n"); - std::exit(0); - } - else { - std::fprintf(stderr, "unknown argument '%s' (see --help)\n", arg.c_str()); - std::exit(1); - } + return opts; } - return opts; -} } // namespace k1sim diff --git a/mujoco/shared/gl/XThreads.hpp b/mujoco/shared/gl/XThreads.hpp index 25f54c7..6b86f8c 100644 --- a/mujoco/shared/gl/XThreads.hpp +++ b/mujoco/shared/gl/XThreads.hpp @@ -6,20 +6,20 @@ // on its own render thread). Without it, concurrent Xlib calls corrupt the heap // ("malloc(): invalid size"). XInitThreads() turns on Xlib's internal locking. #if defined(__linux__) && defined(__has_include) -#if __has_include() -#include + #if __has_include() + #include namespace k1sim { -inline void init_x_threads() { - XInitThreads(); -} + inline void init_x_threads() { + XInitThreads(); + } } // namespace k1sim -#define K1SIM_HAVE_X11 1 -#endif + #define K1SIM_HAVE_X11 1 + #endif #endif #ifndef K1SIM_HAVE_X11 namespace k1sim { -inline void init_x_threads() {} + inline void init_x_threads() {} } // namespace k1sim #endif diff --git a/mujoco/shared/k1/BoosterApi.hpp b/mujoco/shared/k1/BoosterApi.hpp index 8205653..928e6c7 100644 --- a/mujoco/shared/k1/BoosterApi.hpp +++ b/mujoco/shared/k1/BoosterApi.hpp @@ -7,50 +7,50 @@ namespace k1sim::booster { -// DDS topics ("rt/" prefix = ROS2 rmw naming, so ROS2 clients interop directly) -inline constexpr const char* TOPIC_LOW_STATE = "rt/low_state"; -inline constexpr const char* TOPIC_JOINT_CTRL = "rt/joint_ctrl"; -inline constexpr const char* TOPIC_ODOMETER_STATE = "rt/odometer_state"; -inline constexpr const char* TOPIC_FALL_DOWN = "rt/fall_down"; -inline constexpr const char* TOPIC_BATTERY_STATE = "rt/battery_state"; -inline constexpr const char* TOPIC_BUTTON_EVENT = "rt/button_event"; -inline constexpr const char* TOPIC_RPC_REQUEST = "rt/LocoApiTopicReq"; -inline constexpr const char* TOPIC_RPC_RESPONSE = "rt/LocoApiTopicResp"; -// Not an SDK constant: the robot's head pose (geometry_msgs Pose), the topic NUbots' -// K1Sensors subscribes to (K1Sensors.yaml head_pose.topic). -inline constexpr const char* TOPIC_HEAD_POSE = "rt/head_pose"; - -// LocoApiId values carried in RpcReqMsg.header JSON {"api_id": } -enum ApiId : int { - CHANGE_MODE = 2000, // body {"mode": } - MOVE = 2001, // body {"vx":,"vy":,"vyaw":} - ROTATE_HEAD = 2004, // body {"pitch":,"yaw":} - WAVE_HAND = 2005, - ROTATE_HEAD_WITH_DIRECTION = 2006, - LIE_DOWN = 2007, // empty body - GET_UP = 2008, // empty body - GET_MODE = 2017, // response body {"mode": } - GET_UP_WITH_MODE = 2025, // body {"mode": } — what NUbots' BoosterGetUp uses - VISUAL_KICK = 2038, // what NUbots' BoosterVisualKick uses -}; - -// booster::robot::RobotMode -enum RobotMode : int { - UNKNOWN = -1, - DAMPING = 0, // motors damp, robot collapses if unsupported - PREPARE = 1, // stand on both feet - WALKING = 2, // velocity-command locomotion - CUSTOM = 3, // low-level rt/joint_ctrl control - SOCCER = 4, // soccer locomotion — NUbots enters this immediately at startup -}; - -// FallDownState.fall_down_state (matches NUbots' BoosterFallDownState enum) -enum FallState : int { - IS_READY = 0, - IS_FALLING = 1, - HAS_FALLEN = 2, - IS_GETTING_UP = 3, -}; + // DDS topics ("rt/" prefix = ROS2 rmw naming, so ROS2 clients interop directly) + inline constexpr const char* TOPIC_LOW_STATE = "rt/low_state"; + inline constexpr const char* TOPIC_JOINT_CTRL = "rt/joint_ctrl"; + inline constexpr const char* TOPIC_ODOMETER_STATE = "rt/odometer_state"; + inline constexpr const char* TOPIC_FALL_DOWN = "rt/fall_down"; + inline constexpr const char* TOPIC_BATTERY_STATE = "rt/battery_state"; + inline constexpr const char* TOPIC_BUTTON_EVENT = "rt/button_event"; + inline constexpr const char* TOPIC_RPC_REQUEST = "rt/LocoApiTopicReq"; + inline constexpr const char* TOPIC_RPC_RESPONSE = "rt/LocoApiTopicResp"; + // Not an SDK constant: the robot's head pose (geometry_msgs Pose), the topic NUbots' + // K1Sensors subscribes to (K1Sensors.yaml head_pose.topic). + inline constexpr const char* TOPIC_HEAD_POSE = "rt/head_pose"; + + // LocoApiId values carried in RpcReqMsg.header JSON {"api_id": } + enum ApiId : int { + CHANGE_MODE = 2000, // body {"mode": } + MOVE = 2001, // body {"vx":,"vy":,"vyaw":} + ROTATE_HEAD = 2004, // body {"pitch":,"yaw":} + WAVE_HAND = 2005, + ROTATE_HEAD_WITH_DIRECTION = 2006, + LIE_DOWN = 2007, // empty body + GET_UP = 2008, // empty body + GET_MODE = 2017, // response body {"mode": } + GET_UP_WITH_MODE = 2025, // body {"mode": } — what NUbots' BoosterGetUp uses + VISUAL_KICK = 2038, // what NUbots' BoosterVisualKick uses + }; + + // booster::robot::RobotMode + enum RobotMode : int { + UNKNOWN = -1, + DAMPING = 0, // motors damp, robot collapses if unsupported + PREPARE = 1, // stand on both feet + WALKING = 2, // velocity-command locomotion + CUSTOM = 3, // low-level rt/joint_ctrl control + SOCCER = 4, // soccer locomotion — NUbots enters this immediately at startup + }; + + // FallDownState.fall_down_state (matches NUbots' BoosterFallDownState enum) + enum FallState : int { + IS_READY = 0, + IS_FALLING = 1, + HAS_FALLEN = 2, + IS_GETTING_UP = 3, + }; } // namespace k1sim::booster diff --git a/mujoco/shared/k1/JointIndex.hpp b/mujoco/shared/k1/JointIndex.hpp index fbcf2ff..78e4b51 100644 --- a/mujoco/shared/k1/JointIndex.hpp +++ b/mujoco/shared/k1/JointIndex.hpp @@ -6,63 +6,47 @@ namespace k1sim { -// Booster JointIndexK1 order: the index each joint occupies in the SDK's -// LowState::motor_state_serial. This is the T1 JointIndex with the waist removed. -// The parallel-ankle crank indices (15/16, 21/22 on T1) carry the *serial* -// ankle pitch/roll values here, which map 1:1 onto the MJCF's serial ankle joints. -enum JointIndexK1 : std::size_t { - HeadYaw = 0, - HeadPitch, - LeftShoulderPitch, - LeftShoulderRoll, - LeftElbowPitch, - LeftElbowYaw, - RightShoulderPitch, - RightShoulderRoll, - RightElbowPitch, - RightElbowYaw, - LeftHipPitch, - LeftHipRoll, - LeftHipYaw, - LeftKneePitch, - LeftAnklePitch, - LeftAnkleRoll, - RightHipPitch, - RightHipRoll, - RightHipYaw, - RightKneePitch, - RightAnklePitch, - RightAnkleRoll, -}; + // Booster JointIndexK1 order: the index each joint occupies in the SDK's + // LowState::motor_state_serial. This is the T1 JointIndex with the waist removed. + // The parallel-ankle crank indices (15/16, 21/22 on T1) carry the *serial* + // ankle pitch/roll values here, which map 1:1 onto the MJCF's serial ankle joints. + enum JointIndexK1 : std::size_t { + HeadYaw = 0, + HeadPitch, + LeftShoulderPitch, + LeftShoulderRoll, + LeftElbowPitch, + LeftElbowYaw, + RightShoulderPitch, + RightShoulderRoll, + RightElbowPitch, + RightElbowYaw, + LeftHipPitch, + LeftHipRoll, + LeftHipYaw, + LeftKneePitch, + LeftAnklePitch, + LeftAnkleRoll, + RightHipPitch, + RightHipRoll, + RightHipYaw, + RightKneePitch, + RightAnklePitch, + RightAnkleRoll, + }; -inline constexpr std::size_t JOINT_COUNT = 22; + inline constexpr std::size_t JOINT_COUNT = 22; -// Joint *and* actuator names in the vendored booster_assets K1_22dof.xml, -// in JointIndexK1 order (verified identical to booster_deploy's K1_CFG.joint_names). -inline constexpr std::array JOINT_NAMES = { - "AAHead_yaw", - "Head_pitch", - "ALeft_Shoulder_Pitch", - "Left_Shoulder_Roll", - "Left_Elbow_Pitch", - "Left_Elbow_Yaw", - "ARight_Shoulder_Pitch", - "Right_Shoulder_Roll", - "Right_Elbow_Pitch", - "Right_Elbow_Yaw", - "Left_Hip_Pitch", - "Left_Hip_Roll", - "Left_Hip_Yaw", - "Left_Knee_Pitch", - "Left_Ankle_Pitch", - "Left_Ankle_Roll", - "Right_Hip_Pitch", - "Right_Hip_Roll", - "Right_Hip_Yaw", - "Right_Knee_Pitch", - "Right_Ankle_Pitch", - "Right_Ankle_Roll", -}; + // Joint *and* actuator names in the vendored booster_assets K1_22dof.xml, + // in JointIndexK1 order (verified identical to booster_deploy's K1_CFG.joint_names). + inline constexpr std::array JOINT_NAMES = { + "AAHead_yaw", "Head_pitch", "ALeft_Shoulder_Pitch", "Left_Shoulder_Roll", + "Left_Elbow_Pitch", "Left_Elbow_Yaw", "ARight_Shoulder_Pitch", "Right_Shoulder_Roll", + "Right_Elbow_Pitch", "Right_Elbow_Yaw", "Left_Hip_Pitch", "Left_Hip_Roll", + "Left_Hip_Yaw", "Left_Knee_Pitch", "Left_Ankle_Pitch", "Left_Ankle_Roll", + "Right_Hip_Pitch", "Right_Hip_Roll", "Right_Hip_Yaw", "Right_Knee_Pitch", + "Right_Ankle_Pitch", "Right_Ankle_Roll", + }; } // namespace k1sim diff --git a/mujoco/shared/message/Commands.hpp b/mujoco/shared/message/Commands.hpp index 83031d2..ae0a0d8 100644 --- a/mujoco/shared/message/Commands.hpp +++ b/mujoco/shared/message/Commands.hpp @@ -11,52 +11,52 @@ namespace k1sim::message { -struct WalkCommand { // ApiId::MOVE {"vx","vy","vyaw"} — body-frame velocities - double vx = 0.0; - double vy = 0.0; - double vyaw = 0.0; -}; - -struct HeadCommand { // ApiId::ROTATE_HEAD {"pitch","yaw"} — pitch down-positive - double pitch = 0.0; - double yaw = 0.0; -}; - -struct ModeChangeRequest { // ApiId::CHANGE_MODE — booster::RobotMode value - int mode = 0; -}; - -struct GetUpRequest { // ApiId::GET_UP / GET_UP_WITH_MODE — mode to enter afterwards - int target_mode = 4; // booster::RobotMode::SOCCER (what NUbots requests) -}; - -struct LieDownRequest {}; - -struct VisualKickRequest { // ApiId::VISUAL_KICK - bool start = true; - int version = 1; -}; - -struct MotorCmdData { // one LowCmd MotorCmd (serial order) - uint8_t mode = 0; - float q = 0, dq = 0, tau = 0, kp = 0, kd = 0, weight = 0; -}; - -struct LowCmdMessage { // rt/joint_ctrl, only honoured in RobotMode::CUSTOM - int cmd_type = 1; // 0 = PARALLEL (logged + ignored), 1 = SERIAL - std::vector motors; -}; - -// Emitted once by module::Locomotion at startup; consumed by module::Simulation, -// whose physics thread drives controller->step() (see StepController). -struct ControllerHandle { - StepController* controller = nullptr; -}; - -// Emitted by module::Viewer (Backspace key); consumed by module::Simulation, which -// resets mjData to the startup keyframe. Physics-state only: the attached -// StepController keeps its mode/targets (see SimCore::reset). -struct SimResetRequest {}; + struct WalkCommand { // ApiId::MOVE {"vx","vy","vyaw"} — body-frame velocities + double vx = 0.0; + double vy = 0.0; + double vyaw = 0.0; + }; + + struct HeadCommand { // ApiId::ROTATE_HEAD {"pitch","yaw"} — pitch down-positive + double pitch = 0.0; + double yaw = 0.0; + }; + + struct ModeChangeRequest { // ApiId::CHANGE_MODE — booster::RobotMode value + int mode = 0; + }; + + struct GetUpRequest { // ApiId::GET_UP / GET_UP_WITH_MODE — mode to enter afterwards + int target_mode = 4; // booster::RobotMode::SOCCER (what NUbots requests) + }; + + struct LieDownRequest {}; + + struct VisualKickRequest { // ApiId::VISUAL_KICK + bool start = true; + int version = 1; + }; + + struct MotorCmdData { // one LowCmd MotorCmd (serial order) + uint8_t mode = 0; + float q = 0, dq = 0, tau = 0, kp = 0, kd = 0, weight = 0; + }; + + struct LowCmdMessage { // rt/joint_ctrl, only honoured in RobotMode::CUSTOM + int cmd_type = 1; // 0 = PARALLEL (logged + ignored), 1 = SERIAL + std::vector motors; + }; + + // Emitted once by module::Locomotion at startup; consumed by module::Simulation, + // whose physics thread drives controller->step() (see StepController). + struct ControllerHandle { + StepController* controller = nullptr; + }; + + // Emitted by module::Viewer (Backspace key); consumed by module::Simulation, which + // resets mjData to the startup keyframe. Physics-state only: the attached + // StepController keeps its mode/targets (see SimCore::reset). + struct SimResetRequest {}; } // namespace k1sim::message diff --git a/mujoco/shared/message/SimMessages.hpp b/mujoco/shared/message/SimMessages.hpp index 2f37b1f..9a85a8c 100644 --- a/mujoco/shared/message/SimMessages.hpp +++ b/mujoco/shared/message/SimMessages.hpp @@ -14,57 +14,57 @@ namespace k1sim::message { -struct JointState { - double q = 0.0; // rad - double dq = 0.0; // rad/s - double ddq = 0.0; // rad/s^2 - double tau = 0.0; // N·m (actuator force) -}; + struct JointState { + double q = 0.0; // rad + double dq = 0.0; // rad/s + double ddq = 0.0; // rad/s^2 + double tau = 0.0; // N·m (actuator force) + }; -struct ImuData { - std::array quat{1, 0, 0, 0}; // w,x,y,z world->imu - std::array rpy{}; // roll, pitch, yaw (rad) - std::array gyro{}; // rad/s, body frame - std::array acc{}; // m/s^2, body frame, includes gravity -}; + struct ImuData { + std::array quat{1, 0, 0, 0}; // w,x,y,z world->imu + std::array rpy{}; // roll, pitch, yaw (rad) + std::array gyro{}; // rad/s, body frame + std::array acc{}; // m/s^2, body frame, includes gravity + }; -struct BaseState { - double x = 0.0, y = 0.0, z = 0.0; // world frame - std::array quat{1, 0, 0, 0}; // w,x,y,z - std::array lin_vel{}; // world frame - std::array ang_vel{}; // world frame -}; + struct BaseState { + double x = 0.0, y = 0.0, z = 0.0; // world frame + std::array quat{1, 0, 0, 0}; // w,x,y,z + std::array lin_vel{}; // world frame + std::array ang_vel{}; // world frame + }; -// The real robot's head frame (0.08 m above Head_pitch) in the yaw-only base footprint -// frame (see shared/sim/HeadPose.hpp). -struct HeadPose { - bool valid = false; // false if the model has no Head_2 body - std::array position{}; // m - std::array quat{1, 0, 0, 0}; // w,x,y,z -}; + // The real robot's head frame (0.08 m above Head_pitch) in the yaw-only base footprint + // frame (see shared/sim/HeadPose.hpp). + struct HeadPose { + bool valid = false; // false if the model has no Head_2 body + std::array position{}; // m + std::array quat{1, 0, 0, 0}; // w,x,y,z + }; -// Emitted by the physics thread at the LowState cadence (every N steps, 50 Hz). -struct SimStateUpdate { - double sim_time = 0.0; - uint64_t step_count = 0; - std::array joints{}; // JointIndexK1 order - ImuData imu{}; - BaseState base{}; - HeadPose head{}; - int mode = 0; // booster::RobotMode value - int fall_state = 0; // booster::FallState value - bool getting_up = false; - double measured_rtf = 0.0; -}; + // Emitted by the physics thread at the LowState cadence (every N steps, 50 Hz). + struct SimStateUpdate { + double sim_time = 0.0; + uint64_t step_count = 0; + std::array joints{}; // JointIndexK1 order + ImuData imu{}; + BaseState base{}; + HeadPose head{}; + int mode = 0; // booster::RobotMode value + int fall_state = 0; // booster::FallState value + bool getting_up = false; + double measured_rtf = 0.0; + }; -// Emitted once by module::Simulation after the model is loaded. The mutex guards -// mjData; mjModel is immutable after load. Pointers are valid Startup->Shutdown. -struct SimHandles { - const mjModel* model = nullptr; - mjData* data = nullptr; - std::mutex* mutex = nullptr; - std::atomic* measured_rtf = nullptr; -}; + // Emitted once by module::Simulation after the model is loaded. The mutex guards + // mjData; mjModel is immutable after load. Pointers are valid Startup->Shutdown. + struct SimHandles { + const mjModel* model = nullptr; + mjData* data = nullptr; + std::mutex* mutex = nullptr; + std::atomic* measured_rtf = nullptr; + }; } // namespace k1sim::message diff --git a/mujoco/shared/sim/HeadPose.hpp b/mujoco/shared/sim/HeadPose.hpp index 8e39720..4b3ab92 100644 --- a/mujoco/shared/sim/HeadPose.hpp +++ b/mujoco/shared/sim/HeadPose.hpp @@ -7,46 +7,46 @@ namespace k1sim { -// Head pose in the yaw-only base footprint frame, the frame the real robot's rt/head_pose -// is expressed in. NUbots' K1Sensors composes it with its yaw-only odometry (Hwr) to -// recover the true world pose, including torso tilt when fallen. -struct FootprintPose { - std::array position{}; - std::array quat{1, 0, 0, 0}; // w,x,y,z -}; - -// The real robot's head frame sits this far above the Head_pitch joint (the Head_2 body -// origin) along the head's z-axis, near the head's centre of mass (Head_2 inertial z 0.0805). -// NUbots' K1Sensors removes it again with Hhp (translation [0, 0, -0.08], tuned on the robot), -// so publishing the bare Head_2 origin would put NUbots' torso this much too low. -inline constexpr double HEAD_FRAME_ABOVE_PITCH = 0.08; - -// Hrh = (translate(base_x, base_y, 0) * rotz(base_yaw))^-1 * Hwh * translate(0, 0, HEAD_FRAME_ABOVE_PITCH), -// from the Head_2 body's world pose (xpos/xquat) and the root free joint's qpos (xy + wxyz quat). -inline FootprintPose head_in_footprint(const mjtNum head_p[3], - const mjtNum head_q[4], - const mjtNum base_xy[2], - const mjtNum base_q[4]) { - const mjtNum yaw = std::atan2(2.0 * (base_q[0] * base_q[3] + base_q[1] * base_q[2]), - 1.0 - 2.0 * (base_q[2] * base_q[2] + base_q[3] * base_q[3])); - const mjtNum axis[3]{0, 0, 1}; - mjtNum neg_yaw_q[4]; - mju_axisAngle2Quat(neg_yaw_q, axis, -yaw); - const mjtNum rel_w[3]{head_p[0] - base_xy[0], head_p[1] - base_xy[1], head_p[2]}; - mjtNum rel_p[3]; - mju_rotVecQuat(rel_p, rel_w, neg_yaw_q); - mjtNum rel_q[4]; - mju_mulQuat(rel_q, neg_yaw_q, head_q); - - const mjtNum up_h[3]{0, 0, HEAD_FRAME_ABOVE_PITCH}; - mjtNum up_r[3]; - mju_rotVecQuat(up_r, up_h, rel_q); - - FootprintPose pose; - pose.position = {rel_p[0] + up_r[0], rel_p[1] + up_r[1], rel_p[2] + up_r[2]}; - pose.quat = {rel_q[0], rel_q[1], rel_q[2], rel_q[3]}; - return pose; -} + // Head pose in the yaw-only base footprint frame, the frame the real robot's rt/head_pose + // is expressed in. NUbots' K1Sensors composes it with its yaw-only odometry (Hwr) to + // recover the true world pose, including torso tilt when fallen. + struct FootprintPose { + std::array position{}; + std::array quat{1, 0, 0, 0}; // w,x,y,z + }; + + // The real robot's head frame sits this far above the Head_pitch joint (the Head_2 body + // origin) along the head's z-axis, near the head's centre of mass (Head_2 inertial z 0.0805). + // NUbots' K1Sensors removes it again with Hhp (translation [0, 0, -0.08], tuned on the robot), + // so publishing the bare Head_2 origin would put NUbots' torso this much too low. + inline constexpr double HEAD_FRAME_ABOVE_PITCH = 0.08; + + // Hrh = (translate(base_x, base_y, 0) * rotz(base_yaw))^-1 * Hwh * translate(0, 0, HEAD_FRAME_ABOVE_PITCH), + // from the Head_2 body's world pose (xpos/xquat) and the root free joint's qpos (xy + wxyz quat). + inline FootprintPose head_in_footprint(const mjtNum head_p[3], + const mjtNum head_q[4], + const mjtNum base_xy[2], + const mjtNum base_q[4]) { + const mjtNum yaw = std::atan2(2.0 * (base_q[0] * base_q[3] + base_q[1] * base_q[2]), + 1.0 - 2.0 * (base_q[2] * base_q[2] + base_q[3] * base_q[3])); + const mjtNum axis[3]{0, 0, 1}; + mjtNum neg_yaw_q[4]; + mju_axisAngle2Quat(neg_yaw_q, axis, -yaw); + const mjtNum rel_w[3]{head_p[0] - base_xy[0], head_p[1] - base_xy[1], head_p[2]}; + mjtNum rel_p[3]; + mju_rotVecQuat(rel_p, rel_w, neg_yaw_q); + mjtNum rel_q[4]; + mju_mulQuat(rel_q, neg_yaw_q, head_q); + + const mjtNum up_h[3]{0, 0, HEAD_FRAME_ABOVE_PITCH}; + mjtNum up_r[3]; + mju_rotVecQuat(up_r, up_h, rel_q); + + FootprintPose pose; + pose.position = {rel_p[0] + up_r[0], rel_p[1] + up_r[1], rel_p[2] + up_r[2]}; + pose.quat = {rel_q[0], rel_q[1], rel_q[2], rel_q[3]}; + return pose; + } } // namespace k1sim diff --git a/mujoco/shared/sim/ModelMap.hpp b/mujoco/shared/sim/ModelMap.hpp index 7299145..f0c57c6 100644 --- a/mujoco/shared/sim/ModelMap.hpp +++ b/mujoco/shared/sim/ModelMap.hpp @@ -10,64 +10,64 @@ namespace k1sim { -// Resolves the frozen JointIndexK1 ordering against a loaded mjModel: -// joint qpos/dof addresses and actuator ids per joint, plus the root free joint -// and IMU sensor addresses. Built once after model load; immutable afterwards. -struct ModelMap { - std::array qpos_adr{}; // d->qpos index of each joint - std::array dof_adr{}; // d->qvel index of each joint - std::array act_id{}; // actuator id of each joint + // Resolves the frozen JointIndexK1 ordering against a loaded mjModel: + // joint qpos/dof addresses and actuator ids per joint, plus the root free joint + // and IMU sensor addresses. Built once after model load; immutable afterwards. + struct ModelMap { + std::array qpos_adr{}; // d->qpos index of each joint + std::array dof_adr{}; // d->qvel index of each joint + std::array act_id{}; // actuator id of each joint - int root_qpos_adr = -1; // 7 entries: xyz + wxyz quat (free joint) - int root_dof_adr = -1; // 6 entries: linear + angular velocity - int root_body_id = -1; + int root_qpos_adr = -1; // 7 entries: xyz + wxyz quat (free joint) + int root_dof_adr = -1; // 6 entries: linear + angular velocity + int root_body_id = -1; - int imu_site_id = -1; - // sensordata start addresses (-1 if the sensor is absent) - int sens_quat = -1, sens_gyro = -1, sens_acc = -1, sens_linvel = -1; + int imu_site_id = -1; + // sensordata start addresses (-1 if the sensor is absent) + int sens_quat = -1, sens_gyro = -1, sens_acc = -1, sens_linvel = -1; - static ModelMap build(const mjModel* m) { - ModelMap map; - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - const int jnt = mj_name2id(m, mjOBJ_JOINT, JOINT_NAMES[i]); - const int act = mj_name2id(m, mjOBJ_ACTUATOR, JOINT_NAMES[i]); - if (jnt < 0 || act < 0) { - throw std::runtime_error(std::string("model is missing joint/actuator '") + JOINT_NAMES[i] + "'"); + static ModelMap build(const mjModel* m) { + ModelMap map; + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + const int jnt = mj_name2id(m, mjOBJ_JOINT, JOINT_NAMES[i]); + const int act = mj_name2id(m, mjOBJ_ACTUATOR, JOINT_NAMES[i]); + if (jnt < 0 || act < 0) { + throw std::runtime_error(std::string("model is missing joint/actuator '") + JOINT_NAMES[i] + "'"); + } + map.qpos_adr[i] = m->jnt_qposadr[jnt]; + map.dof_adr[i] = m->jnt_dofadr[jnt]; + map.act_id[i] = act; } - map.qpos_adr[i] = m->jnt_qposadr[jnt]; - map.dof_adr[i] = m->jnt_dofadr[jnt]; - map.act_id[i] = act; - } - for (int j = 0; j < m->njnt; ++j) { - if (m->jnt_type[j] == mjJNT_FREE) { - const int body = m->jnt_bodyid[j]; - // the robot's root free joint, not the ball's: it owns the imu site's body chain - if (map.root_qpos_adr < 0 || m->body_subtreemass[body] > m->body_subtreemass[map.root_body_id]) { - map.root_qpos_adr = m->jnt_qposadr[j]; - map.root_dof_adr = m->jnt_dofadr[j]; - map.root_body_id = body; + for (int j = 0; j < m->njnt; ++j) { + if (m->jnt_type[j] == mjJNT_FREE) { + const int body = m->jnt_bodyid[j]; + // the robot's root free joint, not the ball's: it owns the imu site's body chain + if (map.root_qpos_adr < 0 || m->body_subtreemass[body] > m->body_subtreemass[map.root_body_id]) { + map.root_qpos_adr = m->jnt_qposadr[j]; + map.root_dof_adr = m->jnt_dofadr[j]; + map.root_body_id = body; + } } } - } - if (map.root_qpos_adr < 0) { - throw std::runtime_error("model has no free root joint"); - } + if (map.root_qpos_adr < 0) { + throw std::runtime_error("model has no free root joint"); + } - map.imu_site_id = mj_name2id(m, mjOBJ_SITE, "imu"); + map.imu_site_id = mj_name2id(m, mjOBJ_SITE, "imu"); - auto sensor_adr = [m](const char* name) { - const int id = mj_name2id(m, mjOBJ_SENSOR, name); - return id < 0 ? -1 : m->sensor_adr[id]; - }; - map.sens_quat = sensor_adr("orientation"); - map.sens_gyro = sensor_adr("angular-velocity"); - map.sens_acc = sensor_adr("acceleration"); - map.sens_linvel = sensor_adr("linear-velocity"); + auto sensor_adr = [m](const char* name) { + const int id = mj_name2id(m, mjOBJ_SENSOR, name); + return id < 0 ? -1 : m->sensor_adr[id]; + }; + map.sens_quat = sensor_adr("orientation"); + map.sens_gyro = sensor_adr("angular-velocity"); + map.sens_acc = sensor_adr("acceleration"); + map.sens_linvel = sensor_adr("linear-velocity"); - return map; - } -}; + return map; + } + }; } // namespace k1sim diff --git a/mujoco/shared/sim/PdController.hpp b/mujoco/shared/sim/PdController.hpp index c699980..8f30d85 100644 --- a/mujoco/shared/sim/PdController.hpp +++ b/mujoco/shared/sim/PdController.hpp @@ -9,36 +9,36 @@ namespace k1sim { -// Per-joint PD over the 22 actuated joints (JointIndexK1 order). Gains come from -// config/gains.yaml. Torques are clamped to the model's actuator forcerange. -// Used by the Prepare stand, the GetUp script and both locomotion backends. -class PdController { -public: - std::array kp{}; - std::array kd{}; + // Per-joint PD over the 22 actuated joints (JointIndexK1 order). Gains come from + // config/gains.yaml. Torques are clamped to the model's actuator forcerange. + // Used by the Prepare stand, the GetUp script and both locomotion backends. + class PdController { + public: + std::array kp{}; + std::array kd{}; - // ctrl[i] = clamp(kp*(q_ref - q) + kd*(dq_ref - dq) + tau_ff, forcerange) - // Writes d->ctrl for all 22 actuators; q_ref/dq_ref/tau_ff in JointIndexK1 order. - void apply(const mjModel* m, - mjData* d, - const ModelMap& map, - const std::array& q_ref, - const std::array& dq_ref = {}, - const std::array& tau_ff = {}) const { - for (std::size_t i = 0; i < JOINT_COUNT; ++i) { - const double q = d->qpos[map.qpos_adr[i]]; - const double dq = d->qvel[map.dof_adr[i]]; - double tau = kp[i] * (q_ref[i] - q) + kd[i] * (dq_ref[i] - dq) + tau_ff[i]; - const int act = map.act_id[i]; - const double lo = m->actuator_forcerange[2 * act]; - const double hi = m->actuator_forcerange[2 * act + 1]; - if (m->actuator_forcelimited[act] != 0) { - tau = std::clamp(tau, lo, hi); + // ctrl[i] = clamp(kp*(q_ref - q) + kd*(dq_ref - dq) + tau_ff, forcerange) + // Writes d->ctrl for all 22 actuators; q_ref/dq_ref/tau_ff in JointIndexK1 order. + void apply(const mjModel* m, + mjData* d, + const ModelMap& map, + const std::array& q_ref, + const std::array& dq_ref = {}, + const std::array& tau_ff = {}) const { + for (std::size_t i = 0; i < JOINT_COUNT; ++i) { + const double q = d->qpos[map.qpos_adr[i]]; + const double dq = d->qvel[map.dof_adr[i]]; + double tau = kp[i] * (q_ref[i] - q) + kd[i] * (dq_ref[i] - dq) + tau_ff[i]; + const int act = map.act_id[i]; + const double lo = m->actuator_forcerange[2 * act]; + const double hi = m->actuator_forcerange[2 * act + 1]; + if (m->actuator_forcelimited[act] != 0) { + tau = std::clamp(tau, lo, hi); + } + d->ctrl[act] = tau; } - d->ctrl[act] = tau; } - } -}; + }; } // namespace k1sim diff --git a/mujoco/shared/sim/StepController.hpp b/mujoco/shared/sim/StepController.hpp index 6511d7d..66734e5 100644 --- a/mujoco/shared/sim/StepController.hpp +++ b/mujoco/shared/sim/StepController.hpp @@ -5,22 +5,22 @@ namespace k1sim { -// The seam between the physics loop (module::Simulation) and the control logic -// (module::Locomotion). The physics thread calls step() every physics step with -// the sim mutex held, immediately before mj_step(); the implementation writes -// d->ctrl (and, for the kinematic backend, the root free-joint qvel). -// The state accessors are called from other threads and must be lock-free -// (atomics) — they feed GetMode replies and the rt/fall_down publisher. -class StepController { -public: - virtual ~StepController() = default; + // The seam between the physics loop (module::Simulation) and the control logic + // (module::Locomotion). The physics thread calls step() every physics step with + // the sim mutex held, immediately before mj_step(); the implementation writes + // d->ctrl (and, for the kinematic backend, the root free-joint qvel). + // The state accessors are called from other threads and must be lock-free + // (atomics) — they feed GetMode replies and the rt/fall_down publisher. + class StepController { + public: + virtual ~StepController() = default; - virtual void step(const mjModel* m, mjData* d) = 0; + virtual void step(const mjModel* m, mjData* d) = 0; - virtual int mode() const = 0; // booster::RobotMode value - virtual int fall_state() const = 0; // booster::FallState value - virtual bool getting_up() const = 0; -}; + virtual int mode() const = 0; // booster::RobotMode value + virtual int fall_state() const = 0; // booster::FallState value + virtual bool getting_up() const = 0; + }; } // namespace k1sim diff --git a/mujoco/shared/util/Config.hpp b/mujoco/shared/util/Config.hpp index 40c81a0..bf93f6b 100644 --- a/mujoco/shared/util/Config.hpp +++ b/mujoco/shared/util/Config.hpp @@ -11,44 +11,44 @@ namespace k1sim::config { -// Config directory resolution order: --config-dir, $K1SIM_CONFIG_DIR, /config. -inline std::filesystem::path config_dir() { - if (!cli().config_dir.empty()) { - return cli().config_dir; + // Config directory resolution order: --config-dir, $K1SIM_CONFIG_DIR, /config. + inline std::filesystem::path config_dir() { + if (!cli().config_dir.empty()) { + return cli().config_dir; + } + if (const char* env = std::getenv("K1SIM_CONFIG_DIR")) { + return env; + } + return std::filesystem::path(K1SIM_SOURCE_DIR) / "config"; } - if (const char* env = std::getenv("K1SIM_CONFIG_DIR")) { - return env; + + inline YAML::Node load(const std::string& filename) { + return YAML::LoadFile((config_dir() / filename).string()); } - return std::filesystem::path(K1SIM_SOURCE_DIR) / "config"; -} - -inline YAML::Node load(const std::string& filename) { - return YAML::LoadFile((config_dir() / filename).string()); -} - -// Model/asset paths in configs are relative to the mujoco/ source root. -inline std::filesystem::path resolve_path(const std::string& path) { - std::filesystem::path p(path); - return p.is_absolute() ? p : std::filesystem::path(K1SIM_SOURCE_DIR) / p; -} - -// The scene simulation.yaml lists under `fields` for the named field, or for its default `field` -// when none is named. Exits listing the known fields if there is no such field. -inline std::string field_scene(const YAML::Node& sim_cfg, std::string field = "") { - if (field.empty()) { - field = sim_cfg["field"].as(); + + // Model/asset paths in configs are relative to the mujoco/ source root. + inline std::filesystem::path resolve_path(const std::string& path) { + std::filesystem::path p(path); + return p.is_absolute() ? p : std::filesystem::path(K1SIM_SOURCE_DIR) / p; } - const YAML::Node fields = sim_cfg["fields"]; - if (!fields[field]) { - std::string known; - for (const auto& entry : fields) { - known += " " + entry.first.as(); + + // The scene simulation.yaml lists under `fields` for the named field, or for its default `field` + // when none is named. Exits listing the known fields if there is no such field. + inline std::string field_scene(const YAML::Node& sim_cfg, std::string field = "") { + if (field.empty()) { + field = sim_cfg["field"].as(); + } + const YAML::Node fields = sim_cfg["fields"]; + if (!fields[field]) { + std::string known; + for (const auto& entry : fields) { + known += " " + entry.first.as(); + } + std::fprintf(stderr, "unknown field '%s' (known:%s)\n", field.c_str(), known.c_str()); + std::exit(1); } - std::fprintf(stderr, "unknown field '%s' (known:%s)\n", field.c_str(), known.c_str()); - std::exit(1); + return fields[field].as(); } - return fields[field].as(); -} } // namespace k1sim::config diff --git a/mujoco/src/main.cpp b/mujoco/src/main.cpp index 4395fdd..fa4d340 100644 --- a/mujoco/src/main.cpp +++ b/mujoco/src/main.cpp @@ -10,11 +10,11 @@ #include "shared/gl/XThreads.hpp" namespace { -void handle_signal(int /*signum*/) { - if (NUClear::PowerPlant::powerplant != nullptr) { - NUClear::PowerPlant::powerplant->shutdown(); + void handle_signal(int /*signum*/) { + if (NUClear::PowerPlant::powerplant != nullptr) { + NUClear::PowerPlant::powerplant->shutdown(); + } } -} } // namespace int main(int argc, char** argv) { diff --git a/mujoco/test/contract/check_model.py b/mujoco/test/contract/check_model.py index 46f390c..47021e5 100644 --- a/mujoco/test/contract/check_model.py +++ b/mujoco/test/contract/check_model.py @@ -43,7 +43,7 @@ BALL_RADIUS = 0.0785 DROP_BASE_Z = 1.0785 # ball center height for the 1 m drop test (1.0 m above floor) BOUNCE_COEFF = 0.76 -TARGET_RATIO = BOUNCE_COEFF ** 2 # ~0.5776 +TARGET_RATIO = BOUNCE_COEFF**2 # ~0.5776 RATIO_TOLERANCE = 0.15 results = [] # list of (status, message) where status in {"PASS", "FAIL", "WARN"} @@ -97,9 +97,7 @@ def main(): expected_names = [] overall_ok = False - actual_names = [ - mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(model.nu) - ] + actual_names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) for i in range(model.nu)] if model.nu != 22: record("FAIL", f"expected 22 actuators, got {model.nu}") overall_ok = False @@ -208,9 +206,7 @@ def run_ball_drop_test(model, data): bounce_idx = next((i for i in range(1, len(vz)) if vz[i - 1] < 0 <= vz[i]), None) if bounce_idx is None: raise RuntimeError("ball never bounced off the floor within the simulated window") - apex_idx = next( - (i for i in range(bounce_idx + 1, len(vz)) if vz[i - 1] > 0 >= vz[i]), None - ) + apex_idx = next((i for i in range(bounce_idx + 1, len(vz)) if vz[i - 1] > 0 >= vz[i]), None) if apex_idx is None: raise RuntimeError("ball bounced but no subsequent apex was found") @@ -266,8 +262,12 @@ def run_pd_stability_check(model, data): f"PD stability ({sim_time:.0f}s, official gains.yaml kp/kd/ready_pose): " f"height range [{min_z:.3f}, {max_z:.3f}] m (want [0.45, 0.60]), " f"max tilt {max_tilt:.1f} deg (want < 15) -> " - + ("held" if ok else "did NOT hold (expected: joint-space PD has no balance " - "strategy; full stabilization is workstream B's PD/controller milestone)") + + ( + "held" + if ok + else "did NOT hold (expected: joint-space PD has no balance " + "strategy; full stabilization is workstream B's PD/controller milestone)" + ) ) return ok, msg diff --git a/mujoco/test/contract/host_client/sdk_client_check.cpp b/mujoco/test/contract/host_client/sdk_client_check.cpp index 9526669..04e9dbe 100644 --- a/mujoco/test/contract/host_client/sdk_client_check.cpp +++ b/mujoco/test/contract/host_client/sdk_client_check.cpp @@ -43,7 +43,7 @@ #if __has_include() #include #define K1SIM_CHECK_BATTERY 1 - #define K1SIM_SDK_PINNED 1 + #define K1SIM_SDK_PINNED 1 #endif #if __has_include() #include @@ -61,78 +61,78 @@ using namespace booster_interface::msg; namespace { -std::atomic g_low_state_count{0}; -std::atomic g_low_state_plausible{true}; -std::atomic g_last_motor_count{-1}; - -void LowStateHandler(const void* msg) { - const auto* state = static_cast(msg); - g_low_state_count.fetch_add(1, std::memory_order_relaxed); - g_last_motor_count.store(static_cast(state->motor_state_serial().size()), std::memory_order_relaxed); - for (const auto& m : state->motor_state_serial()) { - if (!std::isfinite(m.q()) || !std::isfinite(m.dq()) || !std::isfinite(m.tau_est()) - || std::fabs(m.q()) > 100.0f) { - g_low_state_plausible.store(false, std::memory_order_relaxed); + std::atomic g_low_state_count{0}; + std::atomic g_low_state_plausible{true}; + std::atomic g_last_motor_count{-1}; + + void LowStateHandler(const void* msg) { + const auto* state = static_cast(msg); + g_low_state_count.fetch_add(1, std::memory_order_relaxed); + g_last_motor_count.store(static_cast(state->motor_state_serial().size()), std::memory_order_relaxed); + for (const auto& m : state->motor_state_serial()) { + if (!std::isfinite(m.q()) || !std::isfinite(m.dq()) || !std::isfinite(m.tau_est()) + || std::fabs(m.q()) > 100.0f) { + g_low_state_plausible.store(false, std::memory_order_relaxed); + } } - } - for (float v : state->imu_state().acc()) { - if (!std::isfinite(v)) { - g_low_state_plausible.store(false, std::memory_order_relaxed); + for (float v : state->imu_state().acc()) { + if (!std::isfinite(v)) { + g_low_state_plausible.store(false, std::memory_order_relaxed); + } } } -} -std::atomic g_odom_count{0}; -void OdometerHandler(const void* /*msg*/) { - g_odom_count.fetch_add(1, std::memory_order_relaxed); -} + std::atomic g_odom_count{0}; + void OdometerHandler(const void* /*msg*/) { + g_odom_count.fetch_add(1, std::memory_order_relaxed); + } #ifdef K1SIM_CHECK_HEAD_POSE -std::atomic g_head_pose_count{0}; -std::atomic g_head_pose_z{-1.0}; -std::atomic g_head_pose_finite{true}; -void HeadPoseHandler(const void* msg) { - const auto* pose = static_cast(msg); - const auto& p = pose->position(); - const auto& q = pose->orientation(); - g_head_pose_count.fetch_add(1, std::memory_order_relaxed); - g_head_pose_z.store(p.z(), std::memory_order_relaxed); - for (double v : {p.x(), p.y(), p.z(), q.x(), q.y(), q.z(), q.w()}) { - if (!std::isfinite(v)) { - g_head_pose_finite.store(false, std::memory_order_relaxed); + std::atomic g_head_pose_count{0}; + std::atomic g_head_pose_z{-1.0}; + std::atomic g_head_pose_finite{true}; + void HeadPoseHandler(const void* msg) { + const auto* pose = static_cast(msg); + const auto& p = pose->position(); + const auto& q = pose->orientation(); + g_head_pose_count.fetch_add(1, std::memory_order_relaxed); + g_head_pose_z.store(p.z(), std::memory_order_relaxed); + for (double v : {p.x(), p.y(), p.z(), q.x(), q.y(), q.z(), q.w()}) { + if (!std::isfinite(v)) { + g_head_pose_finite.store(false, std::memory_order_relaxed); + } } } -} #endif #ifdef K1SIM_CHECK_BATTERY -std::atomic g_battery_count{0}; -std::atomic g_battery_soc{-1.0f}; -void BatteryHandler(const void* msg) { - const auto* battery = static_cast(msg); - g_battery_count.fetch_add(1, std::memory_order_relaxed); - g_battery_soc.store(battery->soc(), std::memory_order_relaxed); -} + std::atomic g_battery_count{0}; + std::atomic g_battery_soc{-1.0f}; + void BatteryHandler(const void* msg) { + const auto* battery = static_cast(msg); + g_battery_count.fetch_add(1, std::memory_order_relaxed); + g_battery_soc.store(battery->soc(), std::memory_order_relaxed); + } #endif -int g_failures = 0; + int g_failures = 0; -void check(bool cond, const std::string& what) { - std::printf("[%s] %s\n", cond ? "PASS" : "FAIL", what.c_str()); - if (!cond) { - ++g_failures; + void check(bool cond, const std::string& what) { + std::printf("[%s] %s\n", cond ? "PASS" : "FAIL", what.c_str()); + if (!cond) { + ++g_failures; + } } -} -void timed_rpc(const char* name, const std::function& fn) { - const auto start = std::chrono::steady_clock::now(); - const int32_t ret = fn(); - const auto end = std::chrono::steady_clock::now(); - const double ms = std::chrono::duration(end - start).count(); - std::printf(" %s -> ret=%d, latency=%.2f ms\n", name, ret, ms); - check(ret == 0, std::string(name) + " returned 0"); - check(ms < 1000.0, std::string(name) + " completed within 1000 ms"); -} + void timed_rpc(const char* name, const std::function& fn) { + const auto start = std::chrono::steady_clock::now(); + const int32_t ret = fn(); + const auto end = std::chrono::steady_clock::now(); + const double ms = std::chrono::duration(end - start).count(); + std::printf(" %s -> ret=%d, latency=%.2f ms\n", name, ret, ms); + check(ret == 0, std::string(name) + " returned 0"); + check(ms < 1000.0, std::string(name) + " completed within 1000 ms"); + } } // namespace @@ -196,33 +196,28 @@ int main() { static_cast(g_battery_count.load()), static_cast(g_battery_soc.load())); check(g_battery_count.load() > 0, "rt/battery_state received at least once (1 Hz publisher)"); - check(g_battery_soc.load() > 0.0f && g_battery_soc.load() <= 100.0f, - "rt/battery_state soc in (0, 100]"); + check(g_battery_soc.load() > 0.0f && g_battery_soc.load() <= 100.0f, "rt/battery_state soc in (0, 100]"); #else - std::printf("[SKIP] rt/battery_state checks (SDK build lacks booster/idl/b1/BatteryState.h — " - "set BOOSTER_SDK_ROOT to a pinned-SDK (324946e7) extract to enable)\n"); + std::printf( + "[SKIP] rt/battery_state checks (SDK build lacks booster/idl/b1/BatteryState.h — " + "set BOOSTER_SDK_ROOT to a pinned-SDK (324946e7) extract to enable)\n"); #endif booster::robot::b1::B1LocoClient client; client.Init(); std::printf("RPC round trip (each must return 0 within 1000 ms):\n"); - timed_rpc("ChangeMode(kPrepare)", - [&] { return client.ChangeMode(booster::robot::RobotMode::kPrepare); }); + timed_rpc("ChangeMode(kPrepare)", [&] { return client.ChangeMode(booster::robot::RobotMode::kPrepare); }); // Give the mode change a couple of SimStateUpdate ticks (50 Hz) to propagate // into SdkBridge's cached mode before asking for it back. std::this_thread::sleep_for(std::chrono::milliseconds(500)); { booster::robot::b1::GetModeResponse mode_resp; - const auto start = std::chrono::steady_clock::now(); + const auto start = std::chrono::steady_clock::now(); const int32_t ret = client.GetMode(mode_resp); - const double ms = - std::chrono::duration(std::chrono::steady_clock::now() - start).count(); - std::printf(" GetMode() -> ret=%d, mode=%d, latency=%.2f ms\n", - ret, - static_cast(mode_resp.mode_), - ms); + const double ms = std::chrono::duration(std::chrono::steady_clock::now() - start).count(); + std::printf(" GetMode() -> ret=%d, mode=%d, latency=%.2f ms\n", ret, static_cast(mode_resp.mode_), ms); check(ret == 0, "GetMode() returned 0"); check(ms < 1000.0, "GetMode() completed within 1000 ms"); check(mode_resp.mode_ == booster::robot::RobotMode::kPrepare, diff --git a/mujoco/test/contract/test_sdk_roundtrip.py b/mujoco/test/contract/test_sdk_roundtrip.py index ce09d51..528d501 100755 --- a/mujoco/test/contract/test_sdk_roundtrip.py +++ b/mujoco/test/contract/test_sdk_roundtrip.py @@ -68,18 +68,31 @@ def docker(*args: str, **kwargs) -> subprocess.CompletedProcess: def start_sim(build_dir: str, synthetic: bool) -> None: binary = "./" + sim_binary(build_dir, synthetic) - cmd = [ - "run", "-d", "--rm", - "--name", CONTAINER_NAME, - "--network", "host", "--ipc", "host", - # CRITICAL: run as the invoking user. A root-run sim leaves root-owned - # Fast-DDS SHM segments in the shared /dev/shm that the host client - # can't write to -> host->sim RPC silently times out. See PROTOCOL.md §4. - "--user", f"{os.getuid()}:{os.getgid()}", - "-v", f"{REPO_DIR}:/workspace/NUSim", - "-w", "/workspace/NUSim/mujoco", - IMAGE, - ] + [binary] + ([] if synthetic else ["--headless"]) + cmd = ( + [ + "run", + "-d", + "--rm", + "--name", + CONTAINER_NAME, + "--network", + "host", + "--ipc", + "host", + # CRITICAL: run as the invoking user. A root-run sim leaves root-owned + # Fast-DDS SHM segments in the shared /dev/shm that the host client + # can't write to -> host->sim RPC silently times out. See PROTOCOL.md §4. + "--user", + f"{os.getuid()}:{os.getgid()}", + "-v", + f"{REPO_DIR}:/workspace/NUSim", + "-w", + "/workspace/NUSim/mujoco", + IMAGE, + ] + + [binary] + + ([] if synthetic else ["--headless"]) + ) print(f"[roundtrip] starting sim: {binary}" + (" (synthetic state source)" if synthetic else ""), flush=True) docker(*cmd, check=True, stdout=subprocess.DEVNULL) diff --git a/mujoco/test/unit/CMakeLists.txt b/mujoco/test/unit/CMakeLists.txt index f54cc6b..731ad6c 100644 --- a/mujoco/test/unit/CMakeLists.txt +++ b/mujoco/test/unit/CMakeLists.txt @@ -1,19 +1,14 @@ -# Every test_*.cpp here becomes a ctest target automatically — workstreams add -# test files without touching this CMakeLists (avoids concurrent-edit conflicts). +# Every test_*.cpp here becomes a ctest target automatically — workstreams add test files without touching this +# CMakeLists (avoids concurrent-edit conflicts). file(GLOB test_sources CONFIGURE_DEPENDS "test_*.cpp") foreach(test_src ${test_sources}) - get_filename_component(test_name ${test_src} NAME_WE) - add_executable(${test_name} ${test_src}) - target_link_libraries( - ${test_name} - PRIVATE k1sim_shared - k1sim_module_consolelog - k1sim_module_simulation - k1sim_module_sdkbridge - k1sim_module_locomotion - k1sim_module_viewer - ) - add_test(NAME ${test_name} COMMAND ${test_name}) - set_tests_properties(${test_name} PROPERTIES WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) + get_filename_component(test_name ${test_src} NAME_WE) + add_executable(${test_name} ${test_src}) + target_link_libraries( + ${test_name} PRIVATE k1sim_shared k1sim_module_consolelog k1sim_module_simulation k1sim_module_sdkbridge + k1sim_module_locomotion k1sim_module_viewer + ) + add_test(NAME ${test_name} COMMAND ${test_name}) + set_tests_properties(${test_name} PROPERTIES WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) endforeach() diff --git a/mujoco/test/unit/test_locomotion.cpp b/mujoco/test/unit/test_locomotion.cpp index 94633a1..db50437 100644 --- a/mujoco/test/unit/test_locomotion.cpp +++ b/mujoco/test/unit/test_locomotion.cpp @@ -23,10 +23,9 @@ #include #include #include +#include #include #include - -#include #include #include "module/Locomotion/src/LocoMath.hpp" @@ -45,300 +44,301 @@ namespace booster = k1sim::booster; namespace { -int g_checks = 0; -int g_failures = 0; + int g_checks = 0; + int g_failures = 0; -void check(bool cond, const std::string& msg) { - ++g_checks; - if (cond) { - std::printf(" PASS: %s\n", msg.c_str()); - } - else { - std::printf(" FAIL: %s\n", msg.c_str()); - ++g_failures; + void check(bool cond, const std::string& msg) { + ++g_checks; + if (cond) { + std::printf(" PASS: %s\n", msg.c_str()); + } + else { + std::printf(" FAIL: %s\n", msg.c_str()); + ++g_failures; + } } -} - -double rad2deg(double rad) { - return rad * 180.0 / 3.14159265358979323846; -} -// Loads the real vendored K1 model and adds a floor plane via the mjSpec -// model-editing API (see the file header comment for why not an XML scene). -mjModel* load_test_model() { - const auto path = k1sim::config::resolve_path("models/k1/K1_22dof.xml"); - char error[1024] = {0}; - mjSpec* spec = mj_parseXML(path.string().c_str(), nullptr, error, sizeof(error)); - if (!spec) { - std::fprintf(stderr, "test_locomotion: failed to parse %s: %s\n", path.string().c_str(), error); - std::exit(1); + double rad2deg(double rad) { + return rad * 180.0 / 3.14159265358979323846; } - mjsBody* world = mjs_findBody(spec, "world"); - mjsGeom* floor = mjs_addGeom(world, nullptr); - floor->type = mjGEOM_PLANE; - floor->size[0] = 0.0; - floor->size[1] = 0.0; - floor->size[2] = 0.05; - floor->friction[0] = 0.8; - floor->friction[1] = 0.005; - floor->friction[2] = 0.0001; - - mjModel* m = mj_compile(spec, nullptr); - if (!m) { - std::fprintf(stderr, "test_locomotion: failed to compile model: %s\n", mjs_getError(spec)); + // Loads the real vendored K1 model and adds a floor plane via the mjSpec + // model-editing API (see the file header comment for why not an XML scene). + mjModel* load_test_model() { + const auto path = k1sim::config::resolve_path("models/k1/K1_22dof.xml"); + char error[1024] = {0}; + mjSpec* spec = mj_parseXML(path.string().c_str(), nullptr, error, sizeof(error)); + if (!spec) { + std::fprintf(stderr, "test_locomotion: failed to parse %s: %s\n", path.string().c_str(), error); + std::exit(1); + } + + mjsBody* world = mjs_findBody(spec, "world"); + mjsGeom* floor = mjs_addGeom(world, nullptr); + floor->type = mjGEOM_PLANE; + floor->size[0] = 0.0; + floor->size[1] = 0.0; + floor->size[2] = 0.05; + floor->friction[0] = 0.8; + floor->friction[1] = 0.005; + floor->friction[2] = 0.0001; + + mjModel* m = mj_compile(spec, nullptr); + if (!m) { + std::fprintf(stderr, "test_locomotion: failed to compile model: %s\n", mjs_getError(spec)); + mj_deleteSpec(spec); + std::exit(1); + } mj_deleteSpec(spec); - std::exit(1); + return m; } - mj_deleteSpec(spec); - return m; -} -void reset_to_keyframe(const mjModel* m, mjData* d, const char* keyframe) { - const int key = mj_name2id(m, mjOBJ_KEY, keyframe); - if (key < 0) { - std::fprintf(stderr, "test_locomotion: model has no keyframe '%s'\n", keyframe); - std::exit(1); + void reset_to_keyframe(const mjModel* m, mjData* d, const char* keyframe) { + const int key = mj_name2id(m, mjOBJ_KEY, keyframe); + if (key < 0) { + std::fprintf(stderr, "test_locomotion: model has no keyframe '%s'\n", keyframe); + std::exit(1); + } + mj_resetDataKeyframe(m, d, key); + mj_forward(m, d); } - mj_resetDataKeyframe(m, d, key); - mj_forward(m, d); -} - -YAML::Node locomotion_cfg() { - return k1sim::config::load("locomotion.yaml"); -} -YAML::Node gains_cfg() { - return k1sim::config::load("gains.yaml"); -} -struct BaseState { - double x, y, z; - double tilt_rad; -}; - -BaseState read_base_state(mjData* d, const ModelMap& map) { - BaseState s; - s.x = d->qpos[map.root_qpos_adr + 0]; - s.y = d->qpos[map.root_qpos_adr + 1]; - s.z = d->qpos[map.root_qpos_adr + 2]; - s.tilt_rad = k1sim::module::base_tilt(d, map); - return s; -} + YAML::Node locomotion_cfg() { + return k1sim::config::load("locomotion.yaml"); + } + YAML::Node gains_cfg() { + return k1sim::config::load("gains.yaml"); + } -bool all_finite(const mjData* d, int nq) { - for (int i = 0; i < nq; ++i) { - if (!std::isfinite(d->qpos[i])) { - return false; - } + struct BaseState { + double x, y, z; + double tilt_rad; + }; + + BaseState read_base_state(mjData* d, const ModelMap& map) { + BaseState s; + s.x = d->qpos[map.root_qpos_adr + 0]; + s.y = d->qpos[map.root_qpos_adr + 1]; + s.z = d->qpos[map.root_qpos_adr + 2]; + s.tilt_rad = k1sim::module::base_tilt(d, map); + return s; } - return true; -} -// Builds a full-body SERIAL LowCmd targeting `q_ref` with the given gains. -std::vector make_low_cmd(const std::array& q_ref, - const std::array& kp, - const std::array& kd) { - std::vector motors(JOINT_COUNT); - for (std::size_t j = 0; j < JOINT_COUNT; ++j) { - motors[j].mode = 1; - motors[j].q = static_cast(q_ref[j]); - motors[j].kp = static_cast(kp[j]); - motors[j].kd = static_cast(kd[j]); + bool all_finite(const mjData* d, int nq) { + for (int i = 0; i < nq; ++i) { + if (!std::isfinite(d->qpos[i])) { + return false; + } + } + return true; } - return motors; -} -std::array load_joint_array(const YAML::Node& node) { - std::array arr{}; - const auto values = node.as>(); - for (std::size_t i = 0; i < JOINT_COUNT && i < values.size(); ++i) { - arr[i] = values[i]; + // Builds a full-body SERIAL LowCmd targeting `q_ref` with the given gains. + std::vector make_low_cmd(const std::array& q_ref, + const std::array& kp, + const std::array& kd) { + std::vector motors(JOINT_COUNT); + for (std::size_t j = 0; j < JOINT_COUNT; ++j) { + motors[j].mode = 1; + motors[j].q = static_cast(q_ref[j]); + motors[j].kp = static_cast(kp[j]); + motors[j].kd = static_cast(kd[j]); + } + return motors; } - return arr; -} -constexpr double kStandHeightMin = 0.45; -constexpr double kStandHeightMax = 0.62; - -// --------------------------------------------------------------------- -// Test 1: PREPARE from the ready keyframe -- 10s, height/tilt held. -// --------------------------------------------------------------------- -void test_prepare() { - std::printf("test_prepare:\n"); - mjModel* m = load_test_model(); - mjData* d = mj_makeData(m); - reset_to_keyframe(m, d, "ready"); - - LocomotionController controller(locomotion_cfg(), gains_cfg()); - controller.request_mode_change(booster::PREPARE); - - const ModelMap map = ModelMap::build(m); - for (int i = 0; i < 10000; ++i) { // 10 s @ 1 kHz - controller.step(m, d); - mj_step(m, d); + std::array load_joint_array(const YAML::Node& node) { + std::array arr{}; + const auto values = node.as>(); + for (std::size_t i = 0; i < JOINT_COUNT && i < values.size(); ++i) { + arr[i] = values[i]; + } + return arr; } - const BaseState s = read_base_state(d, map); - check(s.z > kStandHeightMin && s.z < kStandHeightMax, - "base height in [0.45,0.62] (z=" + std::to_string(s.z) + ")"); - check(rad2deg(s.tilt_rad) < 10.0, "tilt < 10 deg (tilt=" + std::to_string(rad2deg(s.tilt_rad)) + " deg)"); - check(controller.mode() == booster::PREPARE, "mode() reports PREPARE"); - check(controller.fall_state() == booster::IS_READY, "fall_state()==IS_READY while standing"); + constexpr double kStandHeightMin = 0.45; + constexpr double kStandHeightMax = 0.62; + + // --------------------------------------------------------------------- + // Test 1: PREPARE from the ready keyframe -- 10s, height/tilt held. + // --------------------------------------------------------------------- + void test_prepare() { + std::printf("test_prepare:\n"); + mjModel* m = load_test_model(); + mjData* d = mj_makeData(m); + reset_to_keyframe(m, d, "ready"); + + LocomotionController controller(locomotion_cfg(), gains_cfg()); + controller.request_mode_change(booster::PREPARE); + + const ModelMap map = ModelMap::build(m); + for (int i = 0; i < 10000; ++i) { // 10 s @ 1 kHz + controller.step(m, d); + mj_step(m, d); + } - mj_deleteData(d); - mj_deleteModel(m); -} + const BaseState s = read_base_state(d, map); + check(s.z > kStandHeightMin && s.z < kStandHeightMax, + "base height in [0.45,0.62] (z=" + std::to_string(s.z) + ")"); + check(rad2deg(s.tilt_rad) < 10.0, "tilt < 10 deg (tilt=" + std::to_string(rad2deg(s.tilt_rad)) + " deg)"); + check(controller.mode() == booster::PREPARE, "mode() reports PREPARE"); + check(controller.fall_state() == booster::IS_READY, "fall_state()==IS_READY while standing"); -// --------------------------------------------------------------------- -// Test 2: Head tracking (RotateHead) in PREPARE. -// --------------------------------------------------------------------- -void test_head() { - std::printf("test_head:\n"); - mjModel* m = load_test_model(); - mjData* d = mj_makeData(m); - reset_to_keyframe(m, d, "ready"); - - LocomotionController controller(locomotion_cfg(), gains_cfg()); - controller.request_mode_change(booster::PREPARE); - controller.set_head_command(/*pitch=*/0.3, /*yaw=*/0.4); - - const ModelMap map = ModelMap::build(m); - for (int i = 0; i < 1000; ++i) { // 1 s @ 1 kHz - controller.step(m, d); - mj_step(m, d); + mj_deleteData(d); + mj_deleteModel(m); } - // Tolerances allow for PD steady-state error under gravity: the servo listener - // applies no gravity feed-forward (that used to be the locomotion backends' job; - // a NUbots_K1 policy compensates through its LowCmd tau/kp instead). - const double head_pitch = d->qpos[map.qpos_adr[JointIndexK1::HeadPitch]]; - const double head_yaw = d->qpos[map.qpos_adr[JointIndexK1::HeadYaw]]; - check(std::abs(head_pitch - 0.3) < 0.12, "Head_pitch reaches 0.3 within 1s (q=" + std::to_string(head_pitch) + ")"); - check(std::abs(head_yaw - 0.4) < 0.12, "AAHead_yaw reaches 0.4 within 1s (q=" + std::to_string(head_yaw) + ")"); - - mj_deleteData(d); - mj_deleteModel(m); -} + // --------------------------------------------------------------------- + // Test 2: Head tracking (RotateHead) in PREPARE. + // --------------------------------------------------------------------- + void test_head() { + std::printf("test_head:\n"); + mjModel* m = load_test_model(); + mjData* d = mj_makeData(m); + reset_to_keyframe(m, d, "ready"); + + LocomotionController controller(locomotion_cfg(), gains_cfg()); + controller.request_mode_change(booster::PREPARE); + controller.set_head_command(/*pitch=*/0.3, /*yaw=*/0.4); + + const ModelMap map = ModelMap::build(m); + for (int i = 0; i < 1000; ++i) { // 1 s @ 1 kHz + controller.step(m, d); + mj_step(m, d); + } -// --------------------------------------------------------------------- -// Test 3: CUSTOM mode PD-tracks LowCmd servo targets (the path every -// NUbots_K1 locomotion policy now uses). -// --------------------------------------------------------------------- -void test_custom_low_cmd() { - std::printf("test_custom_low_cmd:\n"); - mjModel* m = load_test_model(); - mjData* d = mj_makeData(m); - reset_to_keyframe(m, d, "ready"); - - const YAML::Node gains = gains_cfg(); - const auto ready_pose = load_joint_array(gains["ready_pose"]); - const auto kp = load_joint_array(gains["kp"]); - const auto kd = load_joint_array(gains["kd"]); - - LocomotionController controller(locomotion_cfg(), gains_cfg()); - controller.request_mode_change(booster::CUSTOM); - - const ModelMap map = ModelMap::build(m); - - // Entering CUSTOM clears any stale LowCmd and PD-holds the entry pose until the - // first command of the session arrives (dead-client protection), so let the mode - // change land before streaming — like a real 50 Hz client. - for (int i = 0; i < 100; ++i) { - controller.step(m, d); - mj_step(m, d); + // Tolerances allow for PD steady-state error under gravity: the servo listener + // applies no gravity feed-forward (that used to be the locomotion backends' job; + // a NUbots_K1 policy compensates through its LowCmd tau/kp instead). + const double head_pitch = d->qpos[map.qpos_adr[JointIndexK1::HeadPitch]]; + const double head_yaw = d->qpos[map.qpos_adr[JointIndexK1::HeadYaw]]; + check(std::abs(head_pitch - 0.3) < 0.12, + "Head_pitch reaches 0.3 within 1s (q=" + std::to_string(head_pitch) + ")"); + check(std::abs(head_yaw - 0.4) < 0.12, "AAHead_yaw reaches 0.4 within 1s (q=" + std::to_string(head_yaw) + ")"); + + mj_deleteData(d); + mj_deleteModel(m); } - // Target: ready pose with a deliberate offset on a few joints. - std::array q_ref = ready_pose; - q_ref[JointIndexK1::HeadYaw] = 0.3; - q_ref[JointIndexK1::LeftShoulderPitch] = 0.4; - q_ref[JointIndexK1::RightElbowPitch] = -0.5; - controller.set_low_cmd(/*SERIAL=*/1, make_low_cmd(q_ref, kp, kd)); + // --------------------------------------------------------------------- + // Test 3: CUSTOM mode PD-tracks LowCmd servo targets (the path every + // NUbots_K1 locomotion policy now uses). + // --------------------------------------------------------------------- + void test_custom_low_cmd() { + std::printf("test_custom_low_cmd:\n"); + mjModel* m = load_test_model(); + mjData* d = mj_makeData(m); + reset_to_keyframe(m, d, "ready"); + + const YAML::Node gains = gains_cfg(); + const auto ready_pose = load_joint_array(gains["ready_pose"]); + const auto kp = load_joint_array(gains["kp"]); + const auto kd = load_joint_array(gains["kd"]); + + LocomotionController controller(locomotion_cfg(), gains_cfg()); + controller.request_mode_change(booster::CUSTOM); + + const ModelMap map = ModelMap::build(m); + + // Entering CUSTOM clears any stale LowCmd and PD-holds the entry pose until the + // first command of the session arrives (dead-client protection), so let the mode + // change land before streaming — like a real 50 Hz client. + for (int i = 0; i < 100; ++i) { + controller.step(m, d); + mj_step(m, d); + } - for (int i = 0; i < 3000; ++i) { // 3 s @ 1 kHz - controller.step(m, d); - mj_step(m, d); - } + // Target: ready pose with a deliberate offset on a few joints. + std::array q_ref = ready_pose; + q_ref[JointIndexK1::HeadYaw] = 0.3; + q_ref[JointIndexK1::LeftShoulderPitch] = 0.4; + q_ref[JointIndexK1::RightElbowPitch] = -0.5; + controller.set_low_cmd(/*SERIAL=*/1, make_low_cmd(q_ref, kp, kd)); - check(controller.mode() == booster::CUSTOM, "mode() reports CUSTOM"); - const double q_head = d->qpos[map.qpos_adr[JointIndexK1::HeadYaw]]; - const double q_shoul = d->qpos[map.qpos_adr[JointIndexK1::LeftShoulderPitch]]; - const double q_elbow = d->qpos[map.qpos_adr[JointIndexK1::RightElbowPitch]]; - // 0.12 tolerance: pure PD against gravity has steady-state error (the LowCmd here - // sends no tau feed-forward; a real policy compensates through training). - check(std::abs(q_head - 0.3) < 0.12, "LowCmd head yaw target tracked (q=" + std::to_string(q_head) + ")"); - check(std::abs(q_shoul - 0.4) < 0.12, - "LowCmd shoulder pitch target tracked (q=" + std::to_string(q_shoul) + ")"); - check(std::abs(q_elbow + 0.5) < 0.12, "LowCmd elbow target tracked (q=" + std::to_string(q_elbow) + ")"); - - const BaseState s = read_base_state(d, map); - check(s.z > kStandHeightMin && s.z < kStandHeightMax, - "still standing under LowCmd control (z=" + std::to_string(s.z) + ")"); - check(all_finite(d, m->nq), "qpos stays finite throughout"); - - // An undersized PARALLEL command must not blow up (warn + hold behaviour). - controller.set_low_cmd(/*PARALLEL=*/0, {}); - for (int i = 0; i < 500; ++i) { - controller.step(m, d); - mj_step(m, d); - } - check(all_finite(d, m->nq), "PARALLEL LowCmd: qpos stays finite (hold behaviour)"); + for (int i = 0; i < 3000; ++i) { // 3 s @ 1 kHz + controller.step(m, d); + mj_step(m, d); + } - mj_deleteData(d); - mj_deleteModel(m); -} + check(controller.mode() == booster::CUSTOM, "mode() reports CUSTOM"); + const double q_head = d->qpos[map.qpos_adr[JointIndexK1::HeadYaw]]; + const double q_shoul = d->qpos[map.qpos_adr[JointIndexK1::LeftShoulderPitch]]; + const double q_elbow = d->qpos[map.qpos_adr[JointIndexK1::RightElbowPitch]]; + // 0.12 tolerance: pure PD against gravity has steady-state error (the LowCmd here + // sends no tau feed-forward; a real policy compensates through training). + check(std::abs(q_head - 0.3) < 0.12, "LowCmd head yaw target tracked (q=" + std::to_string(q_head) + ")"); + check(std::abs(q_shoul - 0.4) < 0.12, + "LowCmd shoulder pitch target tracked (q=" + std::to_string(q_shoul) + ")"); + check(std::abs(q_elbow + 0.5) < 0.12, "LowCmd elbow target tracked (q=" + std::to_string(q_elbow) + ")"); + + const BaseState s = read_base_state(d, map); + check(s.z > kStandHeightMin && s.z < kStandHeightMax, + "still standing under LowCmd control (z=" + std::to_string(s.z) + ")"); + check(all_finite(d, m->nq), "qpos stays finite throughout"); + + // An undersized PARALLEL command must not blow up (warn + hold behaviour). + controller.set_low_cmd(/*PARALLEL=*/0, {}); + for (int i = 0; i < 500; ++i) { + controller.step(m, d); + mj_step(m, d); + } + check(all_finite(d, m->nq), "PARALLEL LowCmd: qpos stays finite (hold behaviour)"); -// --------------------------------------------------------------------- -// Test 4: Fall detection from the lying_front keyframe. -// --------------------------------------------------------------------- -void test_fall_detection() { - std::printf("test_fall_detection:\n"); - mjModel* m = load_test_model(); - mjData* d = mj_makeData(m); - reset_to_keyframe(m, d, "lying_front"); - - LocomotionController controller(locomotion_cfg(), gains_cfg()); - controller.request_mode_change(booster::DAMPING); - - for (int i = 0; i < 1000; ++i) { // 1 s to settle flat - controller.step(m, d); - mj_step(m, d); + mj_deleteData(d); + mj_deleteModel(m); } - check(controller.fall_state() == booster::HAS_FALLEN, "fall_state()==HAS_FALLEN when lying"); - check(!controller.getting_up(), "getting_up() is always false (recovery is a NUbots_K1 policy)"); + // --------------------------------------------------------------------- + // Test 4: Fall detection from the lying_front keyframe. + // --------------------------------------------------------------------- + void test_fall_detection() { + std::printf("test_fall_detection:\n"); + mjModel* m = load_test_model(); + mjData* d = mj_makeData(m); + reset_to_keyframe(m, d, "lying_front"); + + LocomotionController controller(locomotion_cfg(), gains_cfg()); + controller.request_mode_change(booster::DAMPING); + + for (int i = 0; i < 1000; ++i) { // 1 s to settle flat + controller.step(m, d); + mj_step(m, d); + } - mj_deleteData(d); - mj_deleteModel(m); -} + check(controller.fall_state() == booster::HAS_FALLEN, "fall_state()==HAS_FALLEN when lying"); + check(!controller.getting_up(), "getting_up() is always false (recovery is a NUbots_K1 policy)"); -// --------------------------------------------------------------------- -// Test 5: WALKING/SOCCER mode requests are wire-compatible but map to -// PREPARE (the walk policy lives in NUbots_K1 now). -// --------------------------------------------------------------------- -void test_walking_maps_to_prepare() { - std::printf("test_walking_maps_to_prepare:\n"); - mjModel* m = load_test_model(); - mjData* d = mj_makeData(m); - reset_to_keyframe(m, d, "ready"); - - LocomotionController controller(locomotion_cfg(), gains_cfg()); - controller.request_mode_change(booster::SOCCER); - - const ModelMap map = ModelMap::build(m); - for (int i = 0; i < 3000; ++i) { // 3 s @ 1 kHz - controller.step(m, d); - mj_step(m, d); + mj_deleteData(d); + mj_deleteModel(m); } - check(controller.mode() == booster::PREPARE, "ChangeMode(SOCCER) reports PREPARE"); - const BaseState s = read_base_state(d, map); - check(s.z > kStandHeightMin && s.z < kStandHeightMax, - "robot holds the ready pose (z=" + std::to_string(s.z) + ")"); + // --------------------------------------------------------------------- + // Test 5: WALKING/SOCCER mode requests are wire-compatible but map to + // PREPARE (the walk policy lives in NUbots_K1 now). + // --------------------------------------------------------------------- + void test_walking_maps_to_prepare() { + std::printf("test_walking_maps_to_prepare:\n"); + mjModel* m = load_test_model(); + mjData* d = mj_makeData(m); + reset_to_keyframe(m, d, "ready"); + + LocomotionController controller(locomotion_cfg(), gains_cfg()); + controller.request_mode_change(booster::SOCCER); + + const ModelMap map = ModelMap::build(m); + for (int i = 0; i < 3000; ++i) { // 3 s @ 1 kHz + controller.step(m, d); + mj_step(m, d); + } - mj_deleteData(d); - mj_deleteModel(m); -} + check(controller.mode() == booster::PREPARE, "ChangeMode(SOCCER) reports PREPARE"); + const BaseState s = read_base_state(d, map); + check(s.z > kStandHeightMin && s.z < kStandHeightMax, + "robot holds the ready pose (z=" + std::to_string(s.z) + ")"); + + mj_deleteData(d); + mj_deleteModel(m); + } } // namespace diff --git a/mujoco/test/unit/test_model_load.cpp b/mujoco/test/unit/test_model_load.cpp index 9a7e87a..2c5e2ba 100644 --- a/mujoco/test/unit/test_model_load.cpp +++ b/mujoco/test/unit/test_model_load.cpp @@ -20,13 +20,13 @@ namespace { -std::string resolve_test_model_path() { - if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { - return override_path; + std::string resolve_test_model_path() { + if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { + return override_path; + } + auto cfg = k1sim::config::load("simulation.yaml"); + return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); } - auto cfg = k1sim::config::load("simulation.yaml"); - return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); -} } // namespace @@ -34,7 +34,7 @@ int main() { const std::string model_path = resolve_test_model_path(); char error[1024] = {0}; - mjModel* m = mj_loadXML(model_path.c_str(), nullptr, error, sizeof(error)); + mjModel* m = mj_loadXML(model_path.c_str(), nullptr, error, sizeof(error)); if (m == nullptr) { std::fprintf(stderr, "mj_loadXML failed for '%s': %s\n", model_path.c_str(), error); return 1; @@ -61,10 +61,10 @@ int main() { if (ok) { std::printf("test_model_load OK (model: %s, nq=%d, nu=%d, timestep=%.6f)\n", - model_path.c_str(), - m->nq, - m->nu, - m->opt.timestep); + model_path.c_str(), + m->nq, + m->nu, + m->opt.timestep); } mj_deleteModel(m); diff --git a/mujoco/test/unit/test_pd_stand.cpp b/mujoco/test/unit/test_pd_stand.cpp index eeac105..c79bbe6 100644 --- a/mujoco/test/unit/test_pd_stand.cpp +++ b/mujoco/test/unit/test_pd_stand.cpp @@ -35,40 +35,40 @@ namespace { -constexpr double kPi = 3.14159265358979323846; + constexpr double kPi = 3.14159265358979323846; -std::string resolve_test_model_path() { - if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { - return override_path; + std::string resolve_test_model_path() { + if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { + return override_path; + } + auto cfg = k1sim::config::load("simulation.yaml"); + return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); } - auto cfg = k1sim::config::load("simulation.yaml"); - return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); -} -double clamp(double v, double lo, double hi) { - return v < lo ? lo : (v > hi ? hi : v); -} + double clamp(double v, double lo, double hi) { + return v < lo ? lo : (v > hi ? hi : v); + } -// The trunk height NUbots' K1Sensors recovers from rt/head_pose: Hwt = Hrh * Hhp * Htp^-1 with -// Hhp = translate(0, 0, -0.08) and Htp = [Rz(yaw) * Ry(pitch), (0.0056, 0, 0.2149 + 0.033)] -// (NUbots_K1 module/input/K1Sensors: K1Sensors.yaml Hhp, k1_model.hpp compute_Htp). -double k1sensors_trunk_z(const k1sim::message::SimStateUpdate& s) { - mjtNum R_h[9]; // row-major - mju_quat2Mat(R_h, s.head.quat.data()); - - const double yaw = s.joints[k1sim::HeadYaw].q, pitch = s.joints[k1sim::HeadPitch].q; - const double cy = std::cos(yaw), sy = std::sin(yaw), cp = std::cos(pitch), sp = std::sin(pitch); - const double R_tp[3][3] = {{cy * cp, -sy, cy * sp}, {sy * cp, cy, sy * sp}, {-sp, 0.0, cp}}; - const double t_tp[3] = {0.0056, 0.0, 0.2149 + 0.033}; - - // Trunk position in the head frame: Hhp's translation plus Htp^-1's (-R_tp^T t_tp). - double v[3]; - for (int c = 0; c < 3; ++c) { - v[c] = -(R_tp[0][c] * t_tp[0] + R_tp[1][c] * t_tp[1] + R_tp[2][c] * t_tp[2]); + // The trunk height NUbots' K1Sensors recovers from rt/head_pose: Hwt = Hrh * Hhp * Htp^-1 with + // Hhp = translate(0, 0, -0.08) and Htp = [Rz(yaw) * Ry(pitch), (0.0056, 0, 0.2149 + 0.033)] + // (NUbots_K1 module/input/K1Sensors: K1Sensors.yaml Hhp, k1_model.hpp compute_Htp). + double k1sensors_trunk_z(const k1sim::message::SimStateUpdate& s) { + mjtNum R_h[9]; // row-major + mju_quat2Mat(R_h, s.head.quat.data()); + + const double yaw = s.joints[k1sim::HeadYaw].q, pitch = s.joints[k1sim::HeadPitch].q; + const double cy = std::cos(yaw), sy = std::sin(yaw), cp = std::cos(pitch), sp = std::sin(pitch); + const double R_tp[3][3] = {{cy * cp, -sy, cy * sp}, {sy * cp, cy, sy * sp}, {-sp, 0.0, cp}}; + const double t_tp[3] = {0.0056, 0.0, 0.2149 + 0.033}; + + // Trunk position in the head frame: Hhp's translation plus Htp^-1's (-R_tp^T t_tp). + double v[3]; + for (int c = 0; c < 3; ++c) { + v[c] = -(R_tp[0][c] * t_tp[0] + R_tp[1][c] * t_tp[1] + R_tp[2][c] * t_tp[2]); + } + v[2] -= 0.08; + return s.head.position[2] + R_h[6] * v[0] + R_h[7] * v[1] + R_h[8] * v[2]; } - v[2] -= 0.08; - return s.head.position[2] + R_h[6] * v[0] + R_h[7] * v[1] + R_h[8] * v[2]; -} } // namespace @@ -77,8 +77,8 @@ int main() { using k1sim::message::SimStateUpdate; k1sim::SimCore::Config cfg; - cfg.model_path = resolve_test_model_path(); - cfg.rtf = 0.0; // free-run: no pacing sleep + cfg.model_path = resolve_test_model_path(); + cfg.rtf = 0.0; // free-run: no pacing sleep cfg.state_publish_divisor = 1; cfg.resync_threshold = 0.05; // unused in free-run @@ -161,7 +161,7 @@ int main() { return 1; } - const auto wall_start = std::chrono::steady_clock::now(); + const auto wall_start = std::chrono::steady_clock::now(); const auto wall_deadline = wall_start + std::chrono::seconds(60); // generous CI safety net sim.start(); @@ -177,8 +177,8 @@ int main() { sim.stop(); - const double wall_elapsed = std::chrono::duration(wall_end - wall_start).count(); - const uint64_t steps = sim.step_count(); + const double wall_elapsed = std::chrono::duration(wall_end - wall_start).count(); + const uint64_t steps = sim.step_count(); const double steps_per_sec = wall_elapsed > 0.0 ? static_cast(steps) / wall_elapsed : 0.0; bool ok = true; @@ -188,9 +188,9 @@ int main() { } if (height_violation.load()) { std::fprintf(stderr, - "FAIL: base height left [0.45, 0.62] after t=1s (min=%.4f max=%.4f)\n", - min_height_after_1s.load(), - max_height_after_1s.load()); + "FAIL: base height left [0.45, 0.62] after t=1s (min=%.4f max=%.4f)\n", + min_height_after_1s.load(), + max_height_after_1s.load()); ok = false; } if (last_tilt_deg.load() >= 10.0) { diff --git a/mujoco/test/unit/test_rpc_dispatch.cpp b/mujoco/test/unit/test_rpc_dispatch.cpp index 77a022b..17226c5 100644 --- a/mujoco/test/unit/test_rpc_dispatch.cpp +++ b/mujoco/test/unit/test_rpc_dispatch.cpp @@ -11,18 +11,18 @@ namespace { -int failures = 0; + int failures = 0; -void expect(bool cond, const std::string& what) { - if (!cond) { - std::fprintf(stderr, "FAIL: %s\n", what.c_str()); - ++failures; + void expect(bool cond, const std::string& what) { + if (!cond) { + std::fprintf(stderr, "FAIL: %s\n", what.c_str()); + ++failures; + } } -} -std::string header_for(int api_id) { - return "{\"api_id\":" + std::to_string(api_id) + "}"; -} + std::string header_for(int api_id) { + return "{\"api_id\":" + std::to_string(api_id) + "}"; + } } // namespace @@ -43,8 +43,8 @@ int main() { // --- MOVE --- { - auto out = dispatch_rpc(header_for(k1sim::booster::MOVE), R"({"vx":0.1,"vy":-0.2,"vyaw":0.3})", 2, - kUnknownStatus); + auto out = + dispatch_rpc(header_for(k1sim::booster::MOVE), R"({"vx":0.1,"vy":-0.2,"vyaw":0.3})", 2, kUnknownStatus); expect(out.action.kind == RpcActionKind::WALK, "MOVE: action kind"); expect(out.action.walk.vx == 0.1, "MOVE: vx"); expect(out.action.walk.vy == -0.2, "MOVE: vy"); @@ -87,8 +87,8 @@ int main() { { auto out = dispatch_rpc(header_for(k1sim::booster::GET_MODE), "", 4, kUnknownStatus); expect(out.action.kind == RpcActionKind::NONE, "GET_MODE: no action emitted"); - expect(out.response_body == R"({"mode":4})", "GET_MODE: response body echoes current mode (got '" - + out.response_body + "')"); + expect(out.response_body == R"({"mode":4})", + "GET_MODE: response body echoes current mode (got '" + out.response_body + "')"); expect(out.status == 0, "GET_MODE: status 0"); } diff --git a/mujoco/test/unit/test_supervisor.cpp b/mujoco/test/unit/test_supervisor.cpp index dd9823e..bc528e2 100644 --- a/mujoco/test/unit/test_supervisor.cpp +++ b/mujoco/test/unit/test_supervisor.cpp @@ -44,353 +44,353 @@ namespace gc = k1sim::module::supervisor::gc; namespace { -bool g_ok = true; + bool g_ok = true; -void fail(const std::string& what) { - std::fprintf(stderr, "FAIL: %s\n", what.c_str()); - g_ok = false; -} - -bool approx(double a, double b, double eps = 1e-9) { - return std::fabs(a - b) <= eps; -} + void fail(const std::string& what) { + std::fprintf(stderr, "FAIL: %s\n", what.c_str()); + g_ok = false; + } -std::string resolve_test_model_path() { - if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { - return override_path; + bool approx(double a, double b, double eps = 1e-9) { + return std::fabs(a - b) <= eps; } - auto cfg = k1sim::config::load("simulation.yaml"); - return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); -} -bool actions_mention(const std::vector& actions, const std::string& needle) { - for (const auto& a : actions) { - if (a.message.find(needle) != std::string::npos) { - return true; + std::string resolve_test_model_path() { + if (const char* override_path = std::getenv("K1SIM_TEST_MODEL")) { + return override_path; } + auto cfg = k1sim::config::load("simulation.yaml"); + return k1sim::config::resolve_path(k1sim::config::field_scene(cfg)).string(); } - return false; -} -// --- A. Wire-format parsing ------------------------------------------------- -// -// Byte offsets below are computed by hand from GameControllerPacket's field -// order/sizes under #pragma pack(1) (see GameControllerPacket.hpp), *not* -// derived from sizeof()/offsetof() on the struct itself -- the point is to -// catch a struct-definition bug (wrong field order/type/size), so the -// expected layout has to come from an independent source. -void test_wire_format() { - constexpr std::size_t OFF_HEADER = 0; - constexpr std::size_t OFF_VERSION = 4; - constexpr std::size_t OFF_STATE = 10; - constexpr std::size_t OFF_KICKING_TEAM = 13; - constexpr std::size_t OFF_TEAM0 = 18; - constexpr std::size_t TEAM_FIXED_SIZE = 10; // team_id..message_budget - constexpr std::size_t ROBOT_SIZE = 3; - constexpr std::size_t TEAM_SIZE = TEAM_FIXED_SIZE + gc::MAX_NUM_PLAYERS * ROBOT_SIZE; // 70 - constexpr std::size_t PACKET_SIZE = OFF_TEAM0 + 2 * TEAM_SIZE; // 158 - - if (sizeof(gc::GameControllerPacket) != PACKET_SIZE) { - fail("sizeof(GameControllerPacket) = " + std::to_string(sizeof(gc::GameControllerPacket)) - + ", expected " + std::to_string(PACKET_SIZE) + " (pack(1) / field layout drifted)"); - return; - } + bool actions_mention(const std::vector& actions, const std::string& needle) { + for (const auto& a : actions) { + if (a.message.find(needle) != std::string::npos) { + return true; + } + } + return false; + } + + // --- A. Wire-format parsing ------------------------------------------------- + // + // Byte offsets below are computed by hand from GameControllerPacket's field + // order/sizes under #pragma pack(1) (see GameControllerPacket.hpp), *not* + // derived from sizeof()/offsetof() on the struct itself -- the point is to + // catch a struct-definition bug (wrong field order/type/size), so the + // expected layout has to come from an independent source. + void test_wire_format() { + constexpr std::size_t OFF_HEADER = 0; + constexpr std::size_t OFF_VERSION = 4; + constexpr std::size_t OFF_STATE = 10; + constexpr std::size_t OFF_KICKING_TEAM = 13; + constexpr std::size_t OFF_TEAM0 = 18; + constexpr std::size_t TEAM_FIXED_SIZE = 10; // team_id..message_budget + constexpr std::size_t ROBOT_SIZE = 3; + constexpr std::size_t TEAM_SIZE = TEAM_FIXED_SIZE + gc::MAX_NUM_PLAYERS * ROBOT_SIZE; // 70 + constexpr std::size_t PACKET_SIZE = OFF_TEAM0 + 2 * TEAM_SIZE; // 158 + + if (sizeof(gc::GameControllerPacket) != PACKET_SIZE) { + fail("sizeof(GameControllerPacket) = " + std::to_string(sizeof(gc::GameControllerPacket)) + ", expected " + + std::to_string(PACKET_SIZE) + " (pack(1) / field layout drifted)"); + return; + } - std::vector buf(PACKET_SIZE, 0); - buf[OFF_HEADER + 0] = 'R'; - buf[OFF_HEADER + 1] = 'G'; - buf[OFF_HEADER + 2] = 'm'; - buf[OFF_HEADER + 3] = 'e'; - buf[OFF_VERSION] = 20; - buf[OFF_STATE] = static_cast(gc::State::PLAYING); - buf[OFF_KICKING_TEAM] = 42; - buf[OFF_TEAM0 + 0] = 7; // teams[0].team_id - buf[OFF_TEAM0 + TEAM_FIXED_SIZE + 0] = - static_cast(gc::PenaltyState::PICK_UP); // teams[0].players[0].penalty_state - - gc::GameControllerPacket parsed{}; - if (!gc::try_parse(buf.data(), buf.size(), parsed)) { - fail("try_parse rejected a well-formed minimal GC packet"); - return; - } - if (parsed.state != gc::State::PLAYING) { - fail("parsed.state != PLAYING"); - } - if (parsed.kicking_team != 42) { - fail("parsed.kicking_team != 42"); - } - if (parsed.teams[0].team_id != 7) { - fail("parsed.teams[0].team_id != 7"); - } - if (parsed.teams[0].players[0].penalty_state != gc::PenaltyState::PICK_UP) { - fail("parsed.teams[0].players[0].penalty_state != PICK_UP"); - } + std::vector buf(PACKET_SIZE, 0); + buf[OFF_HEADER + 0] = 'R'; + buf[OFF_HEADER + 1] = 'G'; + buf[OFF_HEADER + 2] = 'm'; + buf[OFF_HEADER + 3] = 'e'; + buf[OFF_VERSION] = 20; + buf[OFF_STATE] = static_cast(gc::State::PLAYING); + buf[OFF_KICKING_TEAM] = 42; + buf[OFF_TEAM0 + 0] = 7; // teams[0].team_id + buf[OFF_TEAM0 + TEAM_FIXED_SIZE + 0] = + static_cast(gc::PenaltyState::PICK_UP); // teams[0].players[0].penalty_state + + gc::GameControllerPacket parsed{}; + if (!gc::try_parse(buf.data(), buf.size(), parsed)) { + fail("try_parse rejected a well-formed minimal GC packet"); + return; + } + if (parsed.state != gc::State::PLAYING) { + fail("parsed.state != PLAYING"); + } + if (parsed.kicking_team != 42) { + fail("parsed.kicking_team != 42"); + } + if (parsed.teams[0].team_id != 7) { + fail("parsed.teams[0].team_id != 7"); + } + if (parsed.teams[0].players[0].penalty_state != gc::PenaltyState::PICK_UP) { + fail("parsed.teams[0].players[0].penalty_state != PICK_UP"); + } - // Negative cases: all must be rejected, not crash / not silently parse. - gc::GameControllerPacket dummy{}; - std::vector bad_header = buf; - bad_header[0] = 'X'; - if (gc::try_parse(bad_header.data(), bad_header.size(), dummy)) { - fail("try_parse accepted a bad header"); - } - std::vector bad_version = buf; - bad_version[OFF_VERSION] = 1; - if (gc::try_parse(bad_version.data(), bad_version.size(), dummy)) { - fail("try_parse accepted an unsupported version"); - } - if (gc::try_parse(buf.data(), buf.size() - 1, dummy)) { - fail("try_parse accepted a truncated packet"); - } - if (gc::try_parse(nullptr, 0, dummy)) { - fail("try_parse accepted a null buffer"); + // Negative cases: all must be rejected, not crash / not silently parse. + gc::GameControllerPacket dummy{}; + std::vector bad_header = buf; + bad_header[0] = 'X'; + if (gc::try_parse(bad_header.data(), bad_header.size(), dummy)) { + fail("try_parse accepted a bad header"); + } + std::vector bad_version = buf; + bad_version[OFF_VERSION] = 1; + if (gc::try_parse(bad_version.data(), bad_version.size(), dummy)) { + fail("try_parse accepted an unsupported version"); + } + if (gc::try_parse(buf.data(), buf.size() - 1, dummy)) { + fail("try_parse accepted a truncated packet"); + } + if (gc::try_parse(nullptr, 0, dummy)) { + fail("try_parse accepted a null buffer"); + } + + std::printf("test_wire_format OK (sizeof(GameControllerPacket)=%zu)\n", sizeof(gc::GameControllerPacket)); } - std::printf("test_wire_format OK (sizeof(GameControllerPacket)=%zu)\n", sizeof(gc::GameControllerPacket)); -} + // --- B. Placement primitives ------------------------------------------------ + void test_placement_primitives(const mjModel* m, mjData* d) { + const int ball_body = mj_name2id(m, mjOBJ_BODY, "ball"); + const int ball_geom = mj_name2id(m, mjOBJ_GEOM, "ball"); + if (ball_body < 0 || ball_geom < 0) { + fail("scene is missing the 'ball' body/geom"); + return; + } + const int ball_jnt = m->body_jntadr[ball_body]; + const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; + const int ball_dof_adr = m->jnt_dofadr[ball_jnt]; + const double radius = m->geom_size[3 * ball_geom + 0]; + + // Dirty the ball's velocity so zeroing is actually exercised, not + // trivially true because it started at zero. + for (int i = 0; i < 6; ++i) { + d->qvel[ball_dof_adr + i] = 3.5; + } -// --- B. Placement primitives ------------------------------------------------ -void test_placement_primitives(const mjModel* m, mjData* d) { - const int ball_body = mj_name2id(m, mjOBJ_BODY, "ball"); - const int ball_geom = mj_name2id(m, mjOBJ_GEOM, "ball"); - if (ball_body < 0 || ball_geom < 0) { - fail("scene is missing the 'ball' body/geom"); - return; - } - const int ball_jnt = m->body_jntadr[ball_body]; - const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; - const int ball_dof_adr = m->jnt_dofadr[ball_jnt]; - const double radius = m->geom_size[3 * ball_geom + 0]; - - // Dirty the ball's velocity so zeroing is actually exercised, not - // trivially true because it started at zero. - for (int i = 0; i < 6; ++i) { - d->qvel[ball_dof_adr + i] = 3.5; - } + if (!sup::place_free_body_by_geom_center(m, d, ball_body, ball_geom, 0.0, 0.0, radius)) { + fail("place_free_body_by_geom_center returned false for the ball"); + return; + } - if (!sup::place_free_body_by_geom_center(m, d, ball_body, ball_geom, 0.0, 0.0, radius)) { - fail("place_free_body_by_geom_center returned false for the ball"); - return; - } + // Algebraic check: qpos_xyz + the geom's own local offset must equal the + // requested world point (identity orientation, so no rotation to apply). + const double gx = m->geom_pos[3 * ball_geom + 0]; + const double gy = m->geom_pos[3 * ball_geom + 1]; + const double gz = m->geom_pos[3 * ball_geom + 2]; + const double wx = d->qpos[ball_qpos_adr + 0] + gx; + const double wy = d->qpos[ball_qpos_adr + 1] + gy; + const double wz = d->qpos[ball_qpos_adr + 2] + gz; + if (!approx(wx, 0.0) || !approx(wy, 0.0) || !approx(wz, radius)) { + fail("ball geom-center world position != (0,0,radius): got (" + std::to_string(wx) + ", " + + std::to_string(wy) + ", " + std::to_string(wz) + "), radius=" + std::to_string(radius)); + } - // Algebraic check: qpos_xyz + the geom's own local offset must equal the - // requested world point (identity orientation, so no rotation to apply). - const double gx = m->geom_pos[3 * ball_geom + 0]; - const double gy = m->geom_pos[3 * ball_geom + 1]; - const double gz = m->geom_pos[3 * ball_geom + 2]; - const double wx = d->qpos[ball_qpos_adr + 0] + gx; - const double wy = d->qpos[ball_qpos_adr + 1] + gy; - const double wz = d->qpos[ball_qpos_adr + 2] + gz; - if (!approx(wx, 0.0) || !approx(wy, 0.0) || !approx(wz, radius)) { - fail("ball geom-center world position != (0,0,radius): got (" + std::to_string(wx) + ", " - + std::to_string(wy) + ", " + std::to_string(wz) + "), radius=" + std::to_string(radius)); - } + // Kinematic check: let MuJoCo itself compute geom_xpos from qpos and + // compare against the same target -- this is the actually-meaningful + // physical claim ("the ball ends up at field centre"), not just that our + // own offset arithmetic is self-consistent. + mj_kinematics(m, d); + const double kx = d->geom_xpos[3 * ball_geom + 0]; + const double ky = d->geom_xpos[3 * ball_geom + 1]; + const double kz = d->geom_xpos[3 * ball_geom + 2]; + if (!approx(kx, 0.0, 1e-6) || !approx(ky, 0.0, 1e-6) || !approx(kz, radius, 1e-6)) { + fail("mj_kinematics ball geom_xpos != (0,0,radius): got (" + std::to_string(kx) + ", " + std::to_string(ky) + + ", " + std::to_string(kz) + ")"); + } - // Kinematic check: let MuJoCo itself compute geom_xpos from qpos and - // compare against the same target -- this is the actually-meaningful - // physical claim ("the ball ends up at field centre"), not just that our - // own offset arithmetic is self-consistent. - mj_kinematics(m, d); - const double kx = d->geom_xpos[3 * ball_geom + 0]; - const double ky = d->geom_xpos[3 * ball_geom + 1]; - const double kz = d->geom_xpos[3 * ball_geom + 2]; - if (!approx(kx, 0.0, 1e-6) || !approx(ky, 0.0, 1e-6) || !approx(kz, radius, 1e-6)) { - fail("mj_kinematics ball geom_xpos != (0,0,radius): got (" + std::to_string(kx) + ", " + std::to_string(ky) - + ", " + std::to_string(kz) + ")"); - } + for (int i = 0; i < 6; ++i) { + if (d->qvel[ball_dof_adr + i] != 0.0) { + fail("ball qvel not zeroed by placement (dof " + std::to_string(i) + ")"); + break; + } + } - for (int i = 0; i < 6; ++i) { - if (d->qvel[ball_dof_adr + i] != 0.0) { - fail("ball qvel not zeroed by placement (dof " + std::to_string(i) + ")"); - break; + // Robot root: no geom-offset quirk, qpos_xyz is the requested pose + // directly (see k1_scene_robocup.xml's "kickoff" keyframe, which uses + // this exact convention). + const int trunk_body = mj_name2id(m, mjOBJ_BODY, "Trunk"); + if (trunk_body < 0) { + fail("scene is missing the 'Trunk' body"); + return; + } + const int trunk_jnt = m->body_jntadr[trunk_body]; + const int trunk_qpos_adr = m->jnt_qposadr[trunk_jnt]; + const int trunk_dof_adr = m->jnt_dofadr[trunk_jnt]; + for (int i = 0; i < 6; ++i) { + d->qvel[trunk_dof_adr + i] = -2.0; } - } - // Robot root: no geom-offset quirk, qpos_xyz is the requested pose - // directly (see k1_scene_robocup.xml's "kickoff" keyframe, which uses - // this exact convention). - const int trunk_body = mj_name2id(m, mjOBJ_BODY, "Trunk"); - if (trunk_body < 0) { - fail("scene is missing the 'Trunk' body"); - return; - } - const int trunk_jnt = m->body_jntadr[trunk_body]; - const int trunk_qpos_adr = m->jnt_qposadr[trunk_jnt]; - const int trunk_dof_adr = m->jnt_dofadr[trunk_jnt]; - for (int i = 0; i < 6; ++i) { - d->qvel[trunk_dof_adr + i] = -2.0; - } + const double yaw = 1.5707963267948966; // pi/2 -- avoid relying on M_PI's availability under -std=c++17 + if (!sup::place_free_body(m, d, trunk_body, -1.0, 2.0, 0.6, yaw)) { + fail("place_free_body returned false for Trunk"); + return; + } + const auto q = sup::yaw_to_quat(yaw); + bool pose_ok = approx(d->qpos[trunk_qpos_adr + 0], -1.0) && approx(d->qpos[trunk_qpos_adr + 1], 2.0) + && approx(d->qpos[trunk_qpos_adr + 2], 0.6) && approx(d->qpos[trunk_qpos_adr + 3], q[0]) + && approx(d->qpos[trunk_qpos_adr + 4], q[1]) && approx(d->qpos[trunk_qpos_adr + 5], q[2]) + && approx(d->qpos[trunk_qpos_adr + 6], q[3]); + if (!pose_ok) { + fail("Trunk qpos after place_free_body doesn't match the requested pose"); + } + for (int i = 0; i < 6; ++i) { + if (d->qvel[trunk_dof_adr + i] != 0.0) { + fail("Trunk qvel not zeroed by placement (dof " + std::to_string(i) + ")"); + break; + } + } - const double yaw = 1.5707963267948966; // pi/2 -- avoid relying on M_PI's availability under -std=c++17 - if (!sup::place_free_body(m, d, trunk_body, -1.0, 2.0, 0.6, yaw)) { - fail("place_free_body returned false for Trunk"); - return; - } - const auto q = sup::yaw_to_quat(yaw); - bool pose_ok = approx(d->qpos[trunk_qpos_adr + 0], -1.0) && approx(d->qpos[trunk_qpos_adr + 1], 2.0) - && approx(d->qpos[trunk_qpos_adr + 2], 0.6) && approx(d->qpos[trunk_qpos_adr + 3], q[0]) - && approx(d->qpos[trunk_qpos_adr + 4], q[1]) && approx(d->qpos[trunk_qpos_adr + 5], q[2]) - && approx(d->qpos[trunk_qpos_adr + 6], q[3]); - if (!pose_ok) { - fail("Trunk qpos after place_free_body doesn't match the requested pose"); - } - for (int i = 0; i < 6; ++i) { - if (d->qvel[trunk_dof_adr + i] != 0.0) { - fail("Trunk qvel not zeroed by placement (dof " + std::to_string(i) + ")"); - break; + if (g_ok) { + std::printf("test_placement_primitives OK (ball radius=%.4f)\n", radius); } } - if (g_ok) { - std::printf("test_placement_primitives OK (ball radius=%.4f)\n", radius); - } -} + // --- C. SupervisorLogic end-to-end, real config/supervisor.yaml ------------ + void test_supervisor_logic(const mjModel* m, mjData* d) { + sup::SupervisorConfig cfg = sup::load_config(k1sim::config::load("supervisor.yaml")); + if (cfg.robots.empty()) { + fail("config/supervisor.yaml has no robots[] entries -- test needs at least one"); + return; + } + // SupervisorLogic's constructor takes its config by value (and moves + // that parameter copy into itself) -- passing the `cfg` lvalue below + // copies it, so `cfg` (and this reference into it) stays valid after. + const sup::RobotConfig& robot_cfg = cfg.robots.front(); + + const int ball_body = mj_name2id(m, mjOBJ_BODY, cfg.ball.body.c_str()); + const int ball_geom = mj_name2id(m, mjOBJ_GEOM, cfg.ball.geom.c_str()); + const int robot_body = mj_name2id(m, mjOBJ_BODY, robot_cfg.body.c_str()); + if (ball_body < 0 || ball_geom < 0 || robot_body < 0) { + fail("configured ball/robot body or geom not found in the model"); + return; + } + const int ball_jnt = m->body_jntadr[ball_body]; + const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; + const double radius = m->geom_size[3 * ball_geom + 0]; + const int robot_jnt = m->body_jntadr[robot_body]; + const int robot_qpos_adr = m->jnt_qposadr[robot_jnt]; -// --- C. SupervisorLogic end-to-end, real config/supervisor.yaml ------------ -void test_supervisor_logic(const mjModel* m, mjData* d) { - sup::SupervisorConfig cfg = sup::load_config(k1sim::config::load("supervisor.yaml")); - if (cfg.robots.empty()) { - fail("config/supervisor.yaml has no robots[] entries -- test needs at least one"); - return; - } - // SupervisorLogic's constructor takes its config by value (and moves - // that parameter copy into itself) -- passing the `cfg` lvalue below - // copies it, so `cfg` (and this reference into it) stays valid after. - const sup::RobotConfig& robot_cfg = cfg.robots.front(); - - const int ball_body = mj_name2id(m, mjOBJ_BODY, cfg.ball.body.c_str()); - const int ball_geom = mj_name2id(m, mjOBJ_GEOM, cfg.ball.geom.c_str()); - const int robot_body = mj_name2id(m, mjOBJ_BODY, robot_cfg.body.c_str()); - if (ball_body < 0 || ball_geom < 0 || robot_body < 0) { - fail("configured ball/robot body or geom not found in the model"); - return; - } - const int ball_jnt = m->body_jntadr[ball_body]; - const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; - const double radius = m->geom_size[3 * ball_geom + 0]; - const int robot_jnt = m->body_jntadr[robot_body]; - const int robot_qpos_adr = m->jnt_qposadr[robot_jnt]; - - sup::SupervisorLogic logic(cfg); - - // The acceptance scenario: a single State=PLAYING packet (first packet - // this SupervisorLogic has ever seen -- old state is the UNKNOWN_STATE - // sentinel) must centre the ball. - gc::GameControllerPacket pkt{}; - pkt.header = gc::RECEIVE_HEADER; - pkt.version = gc::SUPPORTED_VERSION; - pkt.state = gc::State::PLAYING; - pkt.kicking_team = 0; - - auto actions = logic.process(m, d, pkt); - - const double bx = d->qpos[ball_qpos_adr + 0] + m->geom_pos[3 * ball_geom + 0]; - const double by = d->qpos[ball_qpos_adr + 1] + m->geom_pos[3 * ball_geom + 1]; - const double bz = d->qpos[ball_qpos_adr + 2] + m->geom_pos[3 * ball_geom + 2]; - if (!approx(bx, cfg.ball.x) || !approx(by, cfg.ball.y) || !approx(bz, radius)) { - fail("SupervisorLogic: State=PLAYING (first packet) did not centre the ball; got (" - + std::to_string(bx) + ", " + std::to_string(by) + ", " + std::to_string(bz) + ")"); - } - if (!actions_mention(actions, "ball")) { - fail("SupervisorLogic: no ball-placement action reported for the kickoff packet"); - } + sup::SupervisorLogic logic(cfg); - // No-op check: mark the ball with a recognisable position that isn't the - // centre, then re-run the *same* State=PLAYING packet (no transition -- - // still PLAYING) and confirm the marker survives untouched. - sup::place_free_body(m, d, ball_body, 5.0, 5.0, 5.0, 0.0); - logic.process(m, d, pkt); - if (!approx(d->qpos[ball_qpos_adr + 0], 5.0) || !approx(d->qpos[ball_qpos_adr + 1], 5.0) - || !approx(d->qpos[ball_qpos_adr + 2], 5.0)) { - fail("SupervisorLogic: a repeated (non-transitioning) PLAYING packet moved the ball (should be a no-op)"); - } + // The acceptance scenario: a single State=PLAYING packet (first packet + // this SupervisorLogic has ever seen -- old state is the UNKNOWN_STATE + // sentinel) must centre the ball. + gc::GameControllerPacket pkt{}; + pkt.header = gc::RECEIVE_HEADER; + pkt.version = gc::SUPPORTED_VERSION; + pkt.state = gc::State::PLAYING; + pkt.kicking_team = 0; + + auto actions = logic.process(m, d, pkt); + + const double bx = d->qpos[ball_qpos_adr + 0] + m->geom_pos[3 * ball_geom + 0]; + const double by = d->qpos[ball_qpos_adr + 1] + m->geom_pos[3 * ball_geom + 1]; + const double bz = d->qpos[ball_qpos_adr + 2] + m->geom_pos[3 * ball_geom + 2]; + if (!approx(bx, cfg.ball.x) || !approx(by, cfg.ball.y) || !approx(bz, radius)) { + fail("SupervisorLogic: State=PLAYING (first packet) did not centre the ball; got (" + std::to_string(bx) + + ", " + std::to_string(by) + ", " + std::to_string(bz) + ")"); + } + if (!actions_mention(actions, "ball")) { + fail("SupervisorLogic: no ball-placement action reported for the kickoff packet"); + } - // Penalty transition: this player becomes penalised -> robot goes to - // penalty_pose; then unpenalised -> back to home_pose. Uses whichever - // packet slot/player index the config actually points at. - gc::Team* team_ptr = nullptr; - if (robot_cfg.team_id >= 0) { - for (auto& t : pkt.teams) { - if (t.team_id == static_cast(robot_cfg.team_id)) { - team_ptr = &t; - break; - } + // No-op check: mark the ball with a recognisable position that isn't the + // centre, then re-run the *same* State=PLAYING packet (no transition -- + // still PLAYING) and confirm the marker survives untouched. + sup::place_free_body(m, d, ball_body, 5.0, 5.0, 5.0, 0.0); + logic.process(m, d, pkt); + if (!approx(d->qpos[ball_qpos_adr + 0], 5.0) || !approx(d->qpos[ball_qpos_adr + 1], 5.0) + || !approx(d->qpos[ball_qpos_adr + 2], 5.0)) { + fail("SupervisorLogic: a repeated (non-transitioning) PLAYING packet moved the ball (should be a no-op)"); } - } - if (team_ptr == nullptr) { - team_ptr = &pkt.teams[static_cast(robot_cfg.team_index)]; - } - gc::Team& team = *team_ptr; - team.players[static_cast(robot_cfg.player_id) - 1].penalty_state = gc::PenaltyState::PICK_UP; - - logic.process(m, d, pkt); - bool at_penalty = approx(d->qpos[robot_qpos_adr + 0], robot_cfg.penalty_pose.x) - && approx(d->qpos[robot_qpos_adr + 1], robot_cfg.penalty_pose.y) - && approx(d->qpos[robot_qpos_adr + 2], robot_cfg.penalty_pose.z); - if (!at_penalty) { - fail("SupervisorLogic: penalised robot was not moved to its configured penalty_pose"); - } - team.players[static_cast(robot_cfg.player_id) - 1].penalty_state = gc::PenaltyState::UNPENALISED; - logic.process(m, d, pkt); - bool at_home = approx(d->qpos[robot_qpos_adr + 0], robot_cfg.home_pose.x) - && approx(d->qpos[robot_qpos_adr + 1], robot_cfg.home_pose.y) - && approx(d->qpos[robot_qpos_adr + 2], robot_cfg.home_pose.z); - if (!at_home) { - fail("SupervisorLogic: unpenalised robot was not moved back to its configured home_pose"); - } + // Penalty transition: this player becomes penalised -> robot goes to + // penalty_pose; then unpenalised -> back to home_pose. Uses whichever + // packet slot/player index the config actually points at. + gc::Team* team_ptr = nullptr; + if (robot_cfg.team_id >= 0) { + for (auto& t : pkt.teams) { + if (t.team_id == static_cast(robot_cfg.team_id)) { + team_ptr = &t; + break; + } + } + } + if (team_ptr == nullptr) { + team_ptr = &pkt.teams[static_cast(robot_cfg.team_index)]; + } + gc::Team& team = *team_ptr; + team.players[static_cast(robot_cfg.player_id) - 1].penalty_state = gc::PenaltyState::PICK_UP; - if (g_ok) { - std::printf("test_supervisor_logic OK (robot body '%s', %zu action(s) on kickoff)\n", - robot_cfg.body.c_str(), - actions.size()); - } -} + logic.process(m, d, pkt); + bool at_penalty = approx(d->qpos[robot_qpos_adr + 0], robot_cfg.penalty_pose.x) + && approx(d->qpos[robot_qpos_adr + 1], robot_cfg.penalty_pose.y) + && approx(d->qpos[robot_qpos_adr + 2], robot_cfg.penalty_pose.z); + if (!at_penalty) { + fail("SupervisorLogic: penalised robot was not moved to its configured penalty_pose"); + } -// --- D. Kickoff transition truth table -------------------------------------- -void test_kickoff_transitions(const mjModel* m, mjData* d) { - sup::SupervisorConfig cfg = sup::load_config(k1sim::config::load("supervisor.yaml")); - const int ball_body = mj_name2id(m, mjOBJ_BODY, cfg.ball.body.c_str()); - const int ball_geom = mj_name2id(m, mjOBJ_GEOM, cfg.ball.geom.c_str()); - const int ball_jnt = m->body_jntadr[ball_body]; - const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; - const double radius = m->geom_size[3 * ball_geom + 0]; - - sup::SupervisorLogic logic(cfg); - - auto mark = [&] { sup::place_free_body(m, d, ball_body, -9.0, -9.0, -9.0, 0.0); }; - auto at_centre = [&] { - const double x = d->qpos[ball_qpos_adr + 0] + m->geom_pos[3 * ball_geom + 0]; - const double y = d->qpos[ball_qpos_adr + 1] + m->geom_pos[3 * ball_geom + 1]; - const double z = d->qpos[ball_qpos_adr + 2] + m->geom_pos[3 * ball_geom + 2]; - return approx(x, cfg.ball.x) && approx(y, cfg.ball.y) && approx(z, radius); - }; - - auto step = [&](gc::State state, bool expect_reset, const char* label) { - mark(); - gc::GameControllerPacket pkt{}; - pkt.header = gc::RECEIVE_HEADER; - pkt.version = gc::SUPPORTED_VERSION; - pkt.state = state; + team.players[static_cast(robot_cfg.player_id) - 1].penalty_state = gc::PenaltyState::UNPENALISED; logic.process(m, d, pkt); - const bool reset = at_centre(); - if (reset != expect_reset) { - fail(std::string("kickoff transition '") + label + "': expected reset=" + (expect_reset ? "true" : "false") - + ", got " + (reset ? "true" : "false")); + bool at_home = approx(d->qpos[robot_qpos_adr + 0], robot_cfg.home_pose.x) + && approx(d->qpos[robot_qpos_adr + 1], robot_cfg.home_pose.y) + && approx(d->qpos[robot_qpos_adr + 2], robot_cfg.home_pose.z); + if (!at_home) { + fail("SupervisorLogic: unpenalised robot was not moved back to its configured home_pose"); } - }; - // INITIAL (bootstrap, no reset expected -- entering INITIAL isn't a - // kickoff trigger) -> READY (reset) -> SET (no reset) -> PLAYING (reset). - step(gc::State::INITIAL, false, "start->INITIAL"); - step(gc::State::READY, true, "INITIAL->READY"); - step(gc::State::SET, false, "READY->SET"); - step(gc::State::PLAYING, true, "SET->PLAYING"); - step(gc::State::PLAYING, false, "PLAYING->PLAYING (steady state)"); + if (g_ok) { + std::printf("test_supervisor_logic OK (robot body '%s', %zu action(s) on kickoff)\n", + robot_cfg.body.c_str(), + actions.size()); + } + } - if (g_ok) { - std::printf("test_kickoff_transitions OK\n"); + // --- D. Kickoff transition truth table -------------------------------------- + void test_kickoff_transitions(const mjModel* m, mjData* d) { + sup::SupervisorConfig cfg = sup::load_config(k1sim::config::load("supervisor.yaml")); + const int ball_body = mj_name2id(m, mjOBJ_BODY, cfg.ball.body.c_str()); + const int ball_geom = mj_name2id(m, mjOBJ_GEOM, cfg.ball.geom.c_str()); + const int ball_jnt = m->body_jntadr[ball_body]; + const int ball_qpos_adr = m->jnt_qposadr[ball_jnt]; + const double radius = m->geom_size[3 * ball_geom + 0]; + + sup::SupervisorLogic logic(cfg); + + auto mark = [&] { sup::place_free_body(m, d, ball_body, -9.0, -9.0, -9.0, 0.0); }; + auto at_centre = [&] { + const double x = d->qpos[ball_qpos_adr + 0] + m->geom_pos[3 * ball_geom + 0]; + const double y = d->qpos[ball_qpos_adr + 1] + m->geom_pos[3 * ball_geom + 1]; + const double z = d->qpos[ball_qpos_adr + 2] + m->geom_pos[3 * ball_geom + 2]; + return approx(x, cfg.ball.x) && approx(y, cfg.ball.y) && approx(z, radius); + }; + + auto step = [&](gc::State state, bool expect_reset, const char* label) { + mark(); + gc::GameControllerPacket pkt{}; + pkt.header = gc::RECEIVE_HEADER; + pkt.version = gc::SUPPORTED_VERSION; + pkt.state = state; + logic.process(m, d, pkt); + const bool reset = at_centre(); + if (reset != expect_reset) { + fail(std::string("kickoff transition '") + label + "': expected reset=" + + (expect_reset ? "true" : "false") + ", got " + (reset ? "true" : "false")); + } + }; + + // INITIAL (bootstrap, no reset expected -- entering INITIAL isn't a + // kickoff trigger) -> READY (reset) -> SET (no reset) -> PLAYING (reset). + step(gc::State::INITIAL, false, "start->INITIAL"); + step(gc::State::READY, true, "INITIAL->READY"); + step(gc::State::SET, false, "READY->SET"); + step(gc::State::PLAYING, true, "SET->PLAYING"); + step(gc::State::PLAYING, false, "PLAYING->PLAYING (steady state)"); + + if (g_ok) { + std::printf("test_kickoff_transitions OK\n"); + } } -} } // namespace @@ -399,7 +399,7 @@ int main() { const std::string model_path = resolve_test_model_path(); char error[1024] = {0}; - mjModel* m = mj_loadXML(model_path.c_str(), nullptr, error, sizeof(error)); + mjModel* m = mj_loadXML(model_path.c_str(), nullptr, error, sizeof(error)); if (m == nullptr) { std::fprintf(stderr, "mj_loadXML failed for '%s': %s\n", model_path.c_str(), error); return 1; diff --git a/mujoco/tools/_util.py b/mujoco/tools/_util.py index b4c0971..971f3fa 100644 --- a/mujoco/tools/_util.py +++ b/mujoco/tools/_util.py @@ -1,4 +1,5 @@ """Shared helpers for ./b tool modules.""" + import os import subprocess import sys diff --git a/mujoco/tools/build.py b/mujoco/tools/build.py index c6bf3cb..a4a47fe 100644 --- a/mujoco/tools/build.py +++ b/mujoco/tools/build.py @@ -1,4 +1,5 @@ """./b build [targets] — build the sim (and/or specific role targets) in docker.""" + from _util import k1sim diff --git a/mujoco/tools/configure.py b/mujoco/tools/configure.py index 84aa938..8d6cb23 100644 --- a/mujoco/tools/configure.py +++ b/mujoco/tools/configure.py @@ -9,6 +9,7 @@ Roles are named in path form (sim/soccer) or target form (sim-soccer). After toggling, `./b build` only builds the enabled roles. """ + import os from _util import k1sim @@ -16,14 +17,14 @@ def register(command): command.description = "CMake-configure the sim in docker" - command.add_argument("-i", "--interactive", action="store_true", - help="open the ccmake TUI to toggle roles/options") - command.add_argument("--clean", action="store_true", - help="wipe the build dir (CMakeCache etc.) before configuring") - command.add_argument("--set-role", action="append", default=[], metavar="ROLE", - help="enable a role, e.g. sim/soccer (repeatable)") - command.add_argument("--unset-role", action="append", default=[], metavar="ROLE", - help="disable a role (repeatable)") + command.add_argument("-i", "--interactive", action="store_true", help="open the ccmake TUI to toggle roles/options") + command.add_argument("--clean", action="store_true", help="wipe the build dir (CMakeCache etc.) before configuring") + command.add_argument( + "--set-role", action="append", default=[], metavar="ROLE", help="enable a role, e.g. sim/soccer (repeatable)" + ) + command.add_argument( + "--unset-role", action="append", default=[], metavar="ROLE", help="disable a role (repeatable)" + ) def _role_var(role): diff --git a/mujoco/tools/format.py b/mujoco/tools/format.py index 2c7b999..6f6b53b 100644 --- a/mujoco/tools/format.py +++ b/mujoco/tools/format.py @@ -84,6 +84,7 @@ def _tool(name): sys.exit(f"error: {name} not found. Run `uv sync` in {b.repo_dir}.") return found + # The extensions that are handled by the various formatters formatters = OrderedDict() formatters["clang-format"] = { diff --git a/mujoco/tools/image.py b/mujoco/tools/image.py index 7ba9709..f8f5722 100644 --- a/mujoco/tools/image.py +++ b/mujoco/tools/image.py @@ -3,6 +3,7 @@ Unconditional: use this after bumping a baked dependency (e.g. MuJoCo version) — `./b build` only builds the image when it's *missing*, so a stale image needs this. """ + from _util import k1sim diff --git a/mujoco/tools/roles.py b/mujoco/tools/roles.py index 2b84552..74efb56 100644 --- a/mujoco/tools/roles.py +++ b/mujoco/tools/roles.py @@ -3,6 +3,7 @@ Reads mujoco/roles/**/*.role and, if a build exists, the ROLE_* state from its CMakeCache. Toggle with `./b configure --set-role/--unset-role`, or `-i` for ccmake. """ + import glob import os @@ -26,6 +27,6 @@ def run(**kwargs): print("roles (./b run ):") for f in sorted(glob.glob(os.path.join(roles_dir, "**", "*.role"), recursive=True)): - rel = os.path.relpath(f, roles_dir)[:-5] # sim/soccer + rel = os.path.relpath(f, roles_dir)[:-5] # sim/soccer var = "ROLE_" + rel.replace("/", "-") print(f" {rel:20} {state.get(var, 'ON (default)')}") diff --git a/mujoco/tools/run.py b/mujoco/tools/run.py index 44a9379..8bf4140 100644 --- a/mujoco/tools/run.py +++ b/mujoco/tools/run.py @@ -4,6 +4,7 @@ after the role (including flags like --headless / --model / --rtf) passes straight through to the binary — argparse.REMAINDER, so ./b doesn't try to parse them. """ + import argparse from _util import k1sim diff --git a/mujoco/tools/walk_surface_sweep.py b/mujoco/tools/walk_surface_sweep.py index 8f0edf4..efd2025 100644 --- a/mujoco/tools/walk_surface_sweep.py +++ b/mujoco/tools/walk_surface_sweep.py @@ -42,11 +42,12 @@ import pathlib import sys -import mujoco import numpy as np import onnxruntime as ort import yaml +import mujoco + # JointIndexK1 serial order, matching the actuator order in models/k1/K1_22dof.xml and the # joint arrays in K1WalkPolicy.yaml. JOINT_NAMES = [ @@ -386,9 +387,9 @@ def run_one( writer.writerows(rows) walking = [r for r in rows if r["t"] >= settle_s] - stance = [ - (r["l_cop_x"], r["l_pitch"]) for r in walking if r["l_fz"] > 20.0 - ] + [(r["r_cop_x"], r["r_pitch"]) for r in walking if r["r_fz"] > 20.0] + stance = [(r["l_cop_x"], r["l_pitch"]) for r in walking if r["l_fz"] > 20.0] + [ + (r["r_cop_x"], r["r_pitch"]) for r in walking if r["r_fz"] > 20.0 + ] cops = np.array([c for c, _ in stance]) if stance else np.array([np.nan]) pitches = np.array([p for _, p in stance]) if stance else np.array([np.nan]) end_xy = data.qpos[robot.root_qpos : robot.root_qpos + 2] @@ -448,7 +449,9 @@ def main() -> int: default="true", help="82-obs contract only: how obs[0:3] is produced (default: true)", ) - corruption.add_argument("--odom-rate", type=float, default=50.0, help="odometry publish rate for --linvel-mode aliased") + corruption.add_argument( + "--odom-rate", type=float, default=50.0, help="odometry publish rate for --linvel-mode aliased" + ) corruption.add_argument("--obs-delay", type=float, default=0.0, help="observation transport delay, in 20 ms ticks") corruption.add_argument("--action-delay", type=float, default=0.0, help="command transport delay, in 20 ms ticks") corruption.add_argument("--imu-noise", type=float, default=0.0, help="gaussian sigma on gyro and projected gravity") @@ -470,7 +473,9 @@ def main() -> int: corrupt = Corruption(args) print(f"policy {args.policy.name} obs={obs_dim} scene={args.scene.name} vx={args.vx} m/s") print(f"corruption: {corrupt.describe()} solref=[{args.solref_timeconst}, {args.solref_dampratio}]") - print(f"{'mu':>6} {'fell':>5} {'fall_t':>7} {'dist':>6} {'cop_x':>7} {'cop_p95':>8} {'sole_p':>7} {'sat':>6} {'knee':>6}") + print( + f"{'mu':>6} {'fell':>5} {'fall_t':>7} {'dist':>6} {'cop_x':>7} {'cop_p95':>8} {'sole_p':>7} {'sat':>6} {'knee':>6}" + ) results = [] for mu in args.friction: trace = args.trace_dir / f"walk_mu{mu:.2f}.csv" if args.trace_dir else None