diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000..a807d2b --- /dev/null +++ b/.dockerignore @@ -0,0 +1,3 @@ +* +!docker/ +!docker/schedulestream-v1.requirements.txt diff --git a/.gitignore b/.gitignore index 16c0075..c5e0625 100644 --- a/.gitignore +++ b/.gitignore @@ -218,3 +218,8 @@ __marimo__/ # Streamlit .streamlit/secrets.toml + +agentic_design/ + +# Local autonomous-generation artifacts +datasets/autonomous_runs/ diff --git a/docker/Dockerfile.schedulestream_v1 b/docker/Dockerfile.schedulestream_v1 new file mode 100644 index 0000000..811e5e3 --- /dev/null +++ b/docker/Dockerfile.schedulestream_v1 @@ -0,0 +1,54 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Local-only ScheduleStream/cuRobo-v1 development overlay. ScheduleStream is supplied as a named +# build context because its restricted license does not permit AutoData to vendor or fetch it. +# Build with docker/build_schedulestream_v1.sh; never use the frozen UMI image tag as the base/tag. +ARG BASE_IMAGE=isaac_autodata:curobo +FROM ${BASE_IMAGE} + +USER root + +ARG SCHEDULESTREAM_COMMIT +ARG SCHEDULESTREAM_VERSION +ARG BASE_IMAGE_ID +RUN test -n "${SCHEDULESTREAM_COMMIT}" \ + && test -n "${SCHEDULESTREAM_VERSION}" \ + && test -n "${BASE_IMAGE_ID}" + +LABEL org.opencontainers.image.title="Isaac AutoData ScheduleStream cuRobo-v1 development overlay" \ + org.opencontainers.image.description="Local research/evaluation image; not approved for redistribution" \ + org.opencontainers.image.schedulestream.commit="${SCHEDULESTREAM_COMMIT}" \ + org.opencontainers.image.schedulestream.application="custream" \ + org.opencontainers.image.curobo.api-generation="v1" \ + org.opencontainers.image.base.digest="${BASE_IMAGE_ID}" + +# ScheduleStream's `custream` extra requires structlog but does not constrain it. Pin a reviewed, +# hash-verified universal wheel rather than mutating the host environment or resolving latest. +COPY docker/schedulestream-v1.requirements.txt /tmp/schedulestream-v1.requirements.txt +RUN /isaac-sim/python.sh -m pip install \ + --disable-pip-version-check \ + --no-cache-dir \ + --only-binary=:all: \ + --require-hashes \ + --requirement /tmp/schedulestream-v1.requirements.txt \ + && rm -f /tmp/schedulestream-v1.requirements.txt + +# The named context is an exact, locally available ScheduleStream checkout. Do not copy `.git` or +# unrelated data into the image. Its commit is verified by the host build script and stamped above. +COPY --from=schedulestream pyproject.toml LICENSE /opt/schedulestream/ +COPY --from=schedulestream src /opt/schedulestream/src +RUN SETUPTOOLS_SCM_PRETEND_VERSION_FOR_SCHEDULESTREAM="${SCHEDULESTREAM_VERSION}" \ + /isaac-sim/python.sh -m pip install \ + --disable-pip-version-check \ + --no-build-isolation \ + --no-deps \ + /opt/schedulestream + +ENV SCHEDULESTREAM_SOURCE_COMMIT=${SCHEDULESTREAM_COMMIT} \ + SCHEDULESTREAM_VERSION=${SCHEDULESTREAM_VERSION} + +# Importing the Isaac integration allocates cuRobo CUDA tensors, so the GPU/App smoke test is run +# after the image build by docker/smoke_schedulestream_v1.py rather than in this CPU-only layer. diff --git a/docker/autonomous_entrypoint.sh b/docker/autonomous_entrypoint.sh new file mode 100755 index 0000000..7dc5d17 --- /dev/null +++ b/docker/autonomous_entrypoint.sh @@ -0,0 +1,106 @@ +#!/bin/bash +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +readonly AUTODATA_HOME="/autodata-home" +readonly AUTODATA_RUN_ROOT="/autonomous-run" +readonly AUTODATA_RUNTIME_ROOT="/tmp/autodata-runtime" +readonly AUTODATA_KIT_CACHE="/isaac-sim/kit/cache" +readonly AUTODATA_OV_CACHE="${AUTODATA_HOME}/.cache/ov" +readonly AUTODATA_WARP_CACHE="${AUTODATA_HOME}/.cache/warp" +readonly AUTODATA_GL_CACHE="${AUTODATA_HOME}/.cache/nvidia/GLCache" +readonly AUTODATA_COMPUTE_CACHE="${AUTODATA_HOME}/.nv/ComputeCache" + +fail() { + echo "error: $*" >&2 + exit 2 +} + +require_positive_id() { + local label="$1" + local value="$2" + [[ "${value}" =~ ^[0-9]+$ ]] || fail "${label} must be a positive integer" + ((value > 0 && value <= 4294967294)) || fail "${label} is outside the supported range" +} + +[[ "${EUID}" -eq 0 ]] || fail "container entrypoint must start as root" +[[ "$#" -gt 0 ]] || fail "container command is required" + +host_uid="${DOCKER_RUN_USER_ID:-}" +host_gid="${DOCKER_RUN_GROUP_ID:-}" +require_positive_id "DOCKER_RUN_USER_ID" "${host_uid}" +require_positive_id "DOCKER_RUN_GROUP_ID" "${host_gid}" + +for required_command in chown cut getent groupadd install setpriv useradd; do + command -v "${required_command}" >/dev/null || fail "required command is unavailable: ${required_command}" +done + +autodata_group="autodata_g${host_gid}" +if ! getent group "${host_gid}" >/dev/null 2>&1; then + getent group "${autodata_group}" >/dev/null 2>&1 && fail "image group-name collision: ${autodata_group}" + groupadd --key GID_MAX=4294967294 --gid "${host_gid}" "${autodata_group}" +fi + +autodata_user="autodata_u${host_uid}" +passwd_entry=$(getent passwd "${host_uid}" || true) +if [[ -z "${passwd_entry}" ]]; then + getent passwd "${autodata_user}" >/dev/null 2>&1 && fail "image user-name collision: ${autodata_user}" + useradd \ + --key UID_MAX=4294967294 \ + --no-create-home \ + --no-log-init \ + --uid "${host_uid}" \ + --gid "${host_gid}" \ + --home-dir "${AUTODATA_HOME}" \ + --shell /bin/bash \ + "${autodata_user}" +else + autodata_user="${passwd_entry%%:*}" +fi + +passwd_entry=$(getent passwd "${host_uid}" || true) +[[ -n "${passwd_entry}" ]] || fail "container passwd lookup failed for host UID" +isaac_group_entry=$(getent group isaac-sim || true) +[[ -n "${isaac_group_entry}" ]] || fail "image does not define the isaac-sim group" +isaac_group_gid=$(printf '%s\n' "${isaac_group_entry}" | cut -d: -f3) +require_positive_id "isaac-sim group ID" "${isaac_group_gid}" + +install -d --mode 0700 --owner "${host_uid}" --group "${host_gid}" \ + "${AUTODATA_HOME}" \ + "${AUTODATA_HOME}/.cache" \ + "${AUTODATA_HOME}/.cache/nvidia" \ + "${AUTODATA_HOME}/.nv" \ + "${AUTODATA_RUNTIME_ROOT}" + +cache_roots=( + "${AUTODATA_KIT_CACHE}" + "${AUTODATA_OV_CACHE}" + "${AUTODATA_WARP_CACHE}" + "${AUTODATA_GL_CACHE}" + "${AUTODATA_COMPUTE_CACHE}" +) +for cache_root in "${cache_roots[@]}"; do + [[ -d "${cache_root}" ]] || fail "cache mount is missing: ${cache_root}" + chown "${host_uid}:${host_gid}" "${cache_root}" +done + +[[ -d "${AUTODATA_RUN_ROOT}" ]] || fail "run-directory mount is missing: ${AUTODATA_RUN_ROOT}" +chown "${host_uid}:${host_gid}" "${AUTODATA_RUN_ROOT}" + +exec setpriv \ + --reuid "${host_uid}" \ + --regid "${host_gid}" \ + --groups "${isaac_group_gid}" \ + --no-new-privs \ + env \ + -u DOCKER_RUN_USER_ID \ + -u DOCKER_RUN_GROUP_ID \ + HOME="${AUTODATA_HOME}" \ + USER="${autodata_user}" \ + LOGNAME="${autodata_user}" \ + XDG_RUNTIME_DIR="${AUTODATA_RUNTIME_ROOT}" \ + "$@" diff --git a/docker/build_schedulestream_v1.sh b/docker/build_schedulestream_v1.sh new file mode 100755 index 0000000..7ce8488 --- /dev/null +++ b/docker/build_schedulestream_v1.sh @@ -0,0 +1,84 @@ +#!/bin/bash +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +EXPECTED_SCHEDULESTREAM_COMMIT="f6351b8db8d7da9cb6ddd6854dbfc3123ab048f5" +SCHEDULESTREAM_VERSION="0.0.0.dev0+f6351b8" +BASE_IMAGE="${BASE_IMAGE:-isaac_autodata:curobo}" +OUTPUT_IMAGE="${OUTPUT_IMAGE:-isaac_autodata:schedulestream-v1}" + +script_dir=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd) +repo_root=$(cd -- "${script_dir}/.." && pwd) +schedulestream_source="${SCHEDULESTREAM_SOURCE:-${repo_root}/../nvplan}" + +if [ ! -d "${schedulestream_source}/.git" ]; then + echo "error: ScheduleStream checkout not found at ${schedulestream_source}" >&2 + echo "set SCHEDULESTREAM_SOURCE to the reviewed local checkout" >&2 + exit 2 +fi + +actual_commit=$(git -C "${schedulestream_source}" rev-parse HEAD) +if [ "${actual_commit}" != "${EXPECTED_SCHEDULESTREAM_COMMIT}" ]; then + echo "error: ScheduleStream checkout is ${actual_commit}" >&2 + echo "expected reviewed commit ${EXPECTED_SCHEDULESTREAM_COMMIT}" >&2 + exit 3 +fi +if [ -n "$(git -C "${schedulestream_source}" status --porcelain)" ]; then + echo "error: ScheduleStream checkout has local changes; refusing a non-reproducible image" >&2 + exit 4 +fi + +base_image_id=$(docker image inspect --format '{{.Id}}' "${BASE_IMAGE}") +if [[ ! "${base_image_id}" =~ ^sha256:[0-9a-f]{64}$ ]]; then + echo "error: could not resolve ${BASE_IMAGE} to an immutable local image ID" >&2 + exit 5 +fi +base_image_pin="isaac-autodata-base-pin:${base_image_id#sha256:}-$$" +build_context="" +cleanup() { + if [[ -n "${build_context:-}" && -d "${build_context}" ]]; then + rm -rf -- "${build_context}" + fi + if [[ -n "${base_image_pin:-}" ]]; then + docker image rm "${base_image_pin}" >/dev/null 2>&1 || true + fi +} +trap cleanup EXIT +docker image tag "${base_image_id}" "${base_image_pin}" +build_context=$(mktemp -d "${TMPDIR:-/tmp}/isaac-autodata-schedulestream.XXXXXXXX") +mkdir -p "${build_context}/schedulestream" +# Materialize only paths tracked by the reviewed commit. The checkout was proven clean above, so +# reading those paths from the worktree preserves any reviewed Git-LFS smudge results while a +# NUL-delimited tree listing excludes ignored files and handles arbitrary tracked path names. +git -C "${schedulestream_source}" ls-tree \ + -r \ + --name-only \ + -z \ + "${actual_commit}" \ + -- \ + pyproject.toml \ + LICENSE \ + src \ + | tar \ + --create \ + --file=- \ + --directory="${schedulestream_source}" \ + --null \ + --files-from=- \ + | tar -xf - -C "${build_context}/schedulestream" + +echo "Building ${OUTPUT_IMAGE} from ${BASE_IMAGE} (${base_image_id})" +echo "ScheduleStream commit: ${actual_commit}" +docker build \ + --build-arg "BASE_IMAGE=${base_image_pin}" \ + --build-arg "BASE_IMAGE_ID=${base_image_id}" \ + --build-arg "SCHEDULESTREAM_COMMIT=${actual_commit}" \ + --build-arg "SCHEDULESTREAM_VERSION=${SCHEDULESTREAM_VERSION}" \ + --build-context "schedulestream=${build_context}/schedulestream" \ + --file "${script_dir}/Dockerfile.schedulestream_v1" \ + --tag "${OUTPUT_IMAGE}" \ + "${repo_root}" diff --git a/docker/run_autonomous_task.sh b/docker/run_autonomous_task.sh new file mode 100755 index 0000000..b242682 --- /dev/null +++ b/docker/run_autonomous_task.sh @@ -0,0 +1,275 @@ +#!/bin/bash +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +readonly DEFAULT_IMAGE="isaac_autodata:schedulestream-v1" +readonly ISAAC_CACHE_VERSION="isaac-sim-5-1" +readonly CONTAINER_REPO="/workspaces/isaac_autodata" +readonly CONTAINER_RUN_ROOT="/autonomous-run" +readonly CONTAINER_TASK="${CONTAINER_RUN_ROOT}/task.yaml" +readonly CONTAINER_HOME="/autodata-home" +readonly CONTAINER_XAUTHORITY="/autodata-xauthority" +readonly MAX_HOST_PATH_LENGTH=4096 +readonly MAX_TASK_BYTES=$((4 * 1024 * 1024)) + +usage() { + cat <<'EOF' +Run one autonomous AutoData task in a reviewed ScheduleStream runtime image. + +Usage: + docker/run_autonomous_task.sh [--gui | --headless] [--run-dir PATH] [--image IMAGE] TASK.yaml + +Options: + --gui Launch the Kit window. Internally this selects Isaac Lab's Kit visualizer. + --headless Run without a window (default). + --run-dir PATH Writable host run directory. By default, create a unique directory + below datasets/autonomous_runs. + --image IMAGE Local reviewed image (default: isaac_autodata:schedulestream-v1). + -h, --help Show this help. + +The caller must explicitly export ACCEPT_EULA=Y. The launcher never accepts the EULA on the +caller's behalf. The repository is mounted read-only; only the printed run directory and isolated, +version-namespaced Docker cache volumes are writable. +EOF +} + +fail() { + echo "error: $*" >&2 + exit 2 +} + +require_safe_mount_path() { + local label="$1" + local value="$2" + [[ -n "${value}" ]] || fail "${label} is empty" + ((${#value} <= MAX_HOST_PATH_LENGTH)) || fail "${label} exceeds ${MAX_HOST_PATH_LENGTH} characters" + [[ "${value}" != *","* ]] || fail "${label} cannot contain a comma" + [[ ! "${value}" =~ [[:cntrl:]] ]] || fail "${label} cannot contain control characters" +} + +gui_auth_dir="" +gui_auth_file="" +cleanup() { + if [[ -n "${gui_auth_file}" ]]; then + rm -f -- "${gui_auth_file}" + fi + if [[ -n "${gui_auth_dir}" ]]; then + rmdir -- "${gui_auth_dir}" 2>/dev/null || true + fi +} +trap cleanup EXIT + +script_dir=$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd) +repo_root=$(cd -- "${script_dir}/.." && pwd) +entrypoint_path="${repo_root}/docker/autonomous_entrypoint.sh" + +gui=false +display_mode="" +image="${DEFAULT_IMAGE}" +requested_run_dir="" +task_argument="" + +while (($# > 0)); do + case "$1" in + --gui) + [[ -z "${display_mode}" || "${display_mode}" == "gui" ]] || fail "--gui conflicts with --headless" + gui=true + display_mode="gui" + shift + ;; + --headless) + [[ -z "${display_mode}" || "${display_mode}" == "headless" ]] || fail "--headless conflicts with --gui" + gui=false + display_mode="headless" + shift + ;; + --run-dir) + (($# >= 2)) || fail "--run-dir requires a path" + requested_run_dir="$2" + shift 2 + ;; + --run-dir=*) + requested_run_dir="${1#*=}" + shift + ;; + --image) + (($# >= 2)) || fail "--image requires an image reference" + image="$2" + shift 2 + ;; + --image=*) + image="${1#*=}" + shift + ;; + -h | --help) + usage + exit 0 + ;; + --) + shift + (($# == 1)) || fail "exactly one task YAML is required after --" + [[ -z "${task_argument}" ]] || fail "task YAML was provided more than once" + task_argument="$1" + shift + ;; + -*) + fail "unknown option: $1" + ;; + *) + [[ -z "${task_argument}" ]] || fail "exactly one task YAML is required" + task_argument="$1" + shift + ;; + esac +done + +[[ -n "${task_argument}" ]] || fail "task YAML is required" +[[ "${ACCEPT_EULA:-}" == "Y" ]] || fail "set ACCEPT_EULA=Y after reviewing the NVIDIA EULA" + +((${#image} <= 255)) || fail "image reference exceeds 255 characters" +[[ "${image}" =~ ^[A-Za-z0-9][A-Za-z0-9._/@:-]*$ ]] || fail "image reference contains unsupported characters" +require_safe_mount_path "repository path" "${repo_root}" +require_safe_mount_path "task path" "${task_argument}" +task_path=$(realpath --canonicalize-existing -- "${task_argument}") || fail "task YAML does not exist" +require_safe_mount_path "resolved task path" "${task_path}" +[[ -f "${task_path}" && -r "${task_path}" ]] || fail "task YAML must be a readable regular file" +case "${task_path,,}" in + *.yaml | *.yml) ;; + *) fail "task input must use a .yaml or .yml suffix" ;; +esac +task_bytes=$(stat --format='%s' -- "${task_path}") +((task_bytes <= MAX_TASK_BYTES)) || fail "task YAML exceeds ${MAX_TASK_BYTES} bytes" + +[[ -f "${entrypoint_path}" && -x "${entrypoint_path}" ]] || fail "container entrypoint is not executable" +command -v docker >/dev/null || fail "docker is not available" +command -v install >/dev/null || fail "install is not available" + +image_id=$(docker image inspect --format '{{.Id}}' "${image}" 2>/dev/null) || fail "Docker image is unavailable: ${image}" +[[ "${image_id}" =~ ^sha256:[0-9a-f]{64}$ ]] || fail "Docker returned an invalid image ID for ${image}" +image_contract=$( + docker image inspect \ + --format '{{index .Config.Labels "org.opencontainers.image.schedulestream.application"}}|{{index .Config.Labels "org.opencontainers.image.curobo.api-generation"}}|{{index .Config.Labels "org.opencontainers.image.schedulestream.commit"}}|{{index .Config.Labels "org.opencontainers.image.base.digest"}}' \ + "${image_id}" 2>/dev/null +) || fail "could not inspect the autonomous runtime image" +IFS='|' read -r schedulestream_application curobo_api schedulestream_commit base_image_digest extra_contract <<<"${image_contract}" +[[ -z "${extra_contract}" ]] || fail "autonomous runtime image labels are malformed" +case "${curobo_api}:${schedulestream_application}" in + v1:custream | v2:custream2) ;; + *) fail "image is not a supported ScheduleStream/cuRobo runtime" ;; +esac +[[ "${schedulestream_commit}" =~ ^[0-9a-f]{40}$ ]] || fail "image has no valid ScheduleStream source identity" +[[ "${base_image_digest}" =~ ^sha256:[0-9a-f]{64}$ ]] || fail "image has no valid base-image identity" + +host_uid=$(id -u) +host_gid=$(id -g) +[[ "${host_uid}" =~ ^[0-9]+$ && "${host_gid}" =~ ^[0-9]+$ ]] || fail "host UID/GID are not numeric" +((host_uid > 0 && host_gid > 0)) || fail "refusing to run the autonomous container as host root" + +if ${gui}; then + [[ "${DISPLAY:-}" =~ ^:([0-9]+)(\.[0-9]+)?$ ]] || fail "--gui requires a local DISPLAY such as :0 or :1" + display_number=$((10#${BASH_REMATCH[1]})) + x11_socket="/tmp/.X11-unix/X${display_number}" + [[ -S "${x11_socket}" ]] || fail "X11 socket is unavailable: ${x11_socket}" + command -v xauth >/dev/null || fail "--gui requires the host xauth command" + + host_xauthority="${XAUTHORITY:-${HOME:-}/.Xauthority}" + host_xauthority=$(realpath --canonicalize-existing -- "${host_xauthority}") \ + || fail "Xauthority file does not exist" + require_safe_mount_path "Xauthority path" "${host_xauthority}" + [[ -f "${host_xauthority}" && -r "${host_xauthority}" ]] || fail "Xauthority must be a readable regular file" + + runtime_temp_root="${XDG_RUNTIME_DIR:-/tmp}" + [[ -d "${runtime_temp_root}" && -w "${runtime_temp_root}" ]] || fail "no writable runtime directory for Xauthority" + gui_auth_dir=$(mktemp -d -- "${runtime_temp_root}/isaac-autodata-xauth.XXXXXXXX") + gui_auth_file="${gui_auth_dir}/Xauthority" + install -m 0600 /dev/null "${gui_auth_file}" + xauth_records=$(xauth -f "${host_xauthority}" nlist "${DISPLAY}") || fail "could not read X11 authorization" + [[ -n "${xauth_records}" ]] || fail "Xauthority has no cookie for ${DISPLAY}" + # FamilyWild lets the single copied cookie authenticate the container hostname. The host's + # complete Xauthority database is never exposed to the container. + printf '%s\n' "${xauth_records}" | sed 's/^..../ffff/' | xauth -f "${gui_auth_file}" nmerge - + [[ -s "${gui_auth_file}" ]] || fail "could not create container X11 authorization" +fi + +umask 077 +if [[ -z "${requested_run_dir}" ]]; then + default_run_root="${repo_root}/datasets/autonomous_runs" + mkdir -p -- "${default_run_root}" + run_dir=$(mktemp -d -- "${default_run_root}/run.XXXXXXXX") +else + require_safe_mount_path "run directory" "${requested_run_dir}" + [[ ! -L "${requested_run_dir}" ]] || fail "run directory cannot be a symbolic link" + if [[ -e "${requested_run_dir}" && ! -d "${requested_run_dir}" ]]; then + fail "run directory exists and is not a directory" + fi + mkdir -p -- "${requested_run_dir}" + run_dir=$(realpath --canonicalize-existing -- "${requested_run_dir}") +fi +require_safe_mount_path "resolved run directory" "${run_dir}" +[[ -d "${run_dir}" && -w "${run_dir}" && -x "${run_dir}" ]] || fail "run directory is not writable" +[[ "$(stat --format='%u' -- "${run_dir}")" == "${host_uid}" ]] || fail "run directory must be owned by the invoking user" +[[ "${run_dir}" != "/" && "${run_dir}" != "${repo_root}" ]] || fail "run directory is too broad" +case "${repo_root}/" in + "${run_dir}/"*) fail "run directory cannot contain the repository" ;; +esac + +task_copy="${run_dir}/task.yaml" +if [[ "${task_path}" != "${task_copy}" ]]; then + [[ ! -e "${task_copy}" && ! -L "${task_copy}" ]] || fail "run directory already contains task.yaml" + install --mode 0400 -- "${task_path}" "${task_copy}" + cmp --silent -- "${task_path}" "${task_copy}" || fail "copied task YAML did not match its source" +else + [[ -f "${task_copy}" && -r "${task_copy}" ]] || fail "run-directory task.yaml must be a readable regular file" + [[ "$(stat --format='%u' -- "${task_copy}")" == "${host_uid}" ]] \ + || fail "run-directory task.yaml must be owned by the invoking user" +fi + +printf 'Run directory: %s\n' "${run_dir}" + +image_digest="${image_id#sha256:}" +cache_namespace="isaac-autodata-${ISAAC_CACHE_VERSION}-${curobo_api}-${image_digest:0:12}-u${host_uid}" +docker_args=( + run + --rm + --gpus all + --shm-size 8g + --security-opt no-new-privileges:true + --user 0:0 + --entrypoint "${CONTAINER_REPO}/docker/autonomous_entrypoint.sh" + --workdir "${CONTAINER_RUN_ROOT}" + --env ACCEPT_EULA + --env "DOCKER_RUN_USER_ID=${host_uid}" + --env "DOCKER_RUN_GROUP_ID=${host_gid}" + --env "PYTHONDONTWRITEBYTECODE=1" + --env "PYTHONPATH=${CONTAINER_REPO}:${CONTAINER_REPO}/submodules/IsaacLab-Arena" + --mount "type=bind,src=${repo_root},dst=${CONTAINER_REPO},readonly" + --mount "type=bind,src=${run_dir},dst=${CONTAINER_RUN_ROOT}" + --mount "type=bind,src=${task_copy},dst=${CONTAINER_TASK},readonly" + --mount "type=volume,src=${cache_namespace}-kit,dst=/isaac-sim/kit/cache" + --mount "type=volume,src=${cache_namespace}-ov,dst=${CONTAINER_HOME}/.cache/ov" + --mount "type=volume,src=${cache_namespace}-warp,dst=${CONTAINER_HOME}/.cache/warp" + --mount "type=volume,src=${cache_namespace}-gl,dst=${CONTAINER_HOME}/.cache/nvidia/GLCache" + --mount "type=volume,src=${cache_namespace}-compute,dst=${CONTAINER_HOME}/.nv/ComputeCache" +) + +runner_args=( + /isaac-sim/python.sh + "${CONTAINER_REPO}/isaac_autodata_examples/generate_task_dataset.py" + "${CONTAINER_TASK}" +) +if ${gui}; then + docker_args+=( + --env "DISPLAY=${DISPLAY}" + --env "XAUTHORITY=${CONTAINER_XAUTHORITY}" + --env "QT_X11_NO_MITSHM=1" + --mount "type=bind,src=/tmp/.X11-unix,dst=/tmp/.X11-unix,readonly" + --mount "type=bind,src=${gui_auth_file},dst=${CONTAINER_XAUTHORITY},readonly" + ) + runner_args+=(--gui) +fi + +docker "${docker_args[@]}" "${image_id}" "${runner_args[@]}" diff --git a/docker/schedulestream-v1.requirements.txt b/docker/schedulestream-v1.requirements.txt new file mode 100644 index 0000000..b09667c --- /dev/null +++ b/docker/schedulestream-v1.requirements.txt @@ -0,0 +1,7 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# PyPI trusted-publisher universal wheel, released 2026-06-06. +structlog==26.1.0 --hash=sha256:e081a26d6c373e6d201eca24eede26d8ffab07f88f477822e679183428d3d91e diff --git a/docker/smoke_schedulestream_v1.py b/docker/smoke_schedulestream_v1.py new file mode 100644 index 0000000..cfc44dd --- /dev/null +++ b/docker/smoke_schedulestream_v1.py @@ -0,0 +1,71 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""GPU/App import smoke for the pinned ScheduleStream cuRobo-v1 development image.""" + +from __future__ import annotations + +import json +import os +from importlib.metadata import version + +from isaaclab.app import AppLauncher + +_EXPECTED_SCHEDULESTREAM_COMMIT = "f6351b8db8d7da9cb6ddd6854dbfc3123ab048f5" +_EXPECTED_SCHEDULESTREAM_VERSION = "0.0.0.dev0+f6351b8" + + +def main() -> None: + """Launch headless Isaac, import the v1 integration, and print machine-readable capabilities.""" + + launcher = AppLauncher({"headless": True}) + try: + from schedulestream.applications.isaaclab.controller import PathController + from schedulestream.applications.isaaclab.planner import Planner + + from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + detect_curobo_runtime, + select_schedulestream_backend, + ) + + capabilities = detect_curobo_runtime() + selection = select_schedulestream_backend("curobo_v1", capabilities) + source_commit = os.environ.get("SCHEDULESTREAM_SOURCE_COMMIT") + expected_version = os.environ.get("SCHEDULESTREAM_VERSION") + installed_version = version("schedulestream") + if source_commit != _EXPECTED_SCHEDULESTREAM_COMMIT: + raise RuntimeError( + f"ScheduleStream source commit {source_commit!r} does not match reviewed " + f"commit {_EXPECTED_SCHEDULESTREAM_COMMIT!r}" + ) + if expected_version != _EXPECTED_SCHEDULESTREAM_VERSION or installed_version != expected_version: + raise RuntimeError( + "ScheduleStream version identity mismatch " + f"(image={expected_version!r}, installed={installed_version!r}, " + f"reviewed={_EXPECTED_SCHEDULESTREAM_VERSION!r})" + ) + print( + json.dumps( + { + "controller_class": PathController.__name__, + "curobo": capabilities.to_dict(), + "planner_class": Planner.__name__, + "schedulestream_source_commit": source_commit, + "schedulestream_version": installed_version, + "selection": { + "application": selection.schedulestream_application, + "motion_backend": selection.motion_backend, + }, + }, + sort_keys=True, + ), + flush=True, + ) + finally: + launcher.app.close() + + +if __name__ == "__main__": + main() diff --git a/isaac_autodata_core/autonomous/__init__.py b/isaac_autodata_core/autonomous/__init__.py new file mode 100644 index 0000000..d76080f --- /dev/null +++ b/isaac_autodata_core/autonomous/__init__.py @@ -0,0 +1,74 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Planner-neutral contracts for source-demo-free episode generation.""" + +from isaac_autodata_core.autonomous.output_transaction import ( + DatasetArtifact, + DatasetCommitUncertainError, + OutputTransaction, + RecordingTargets, + RequestDirectoryAnchor, +) +from isaac_autodata_core.autonomous.run_log import RunLogWriter, RunLogWriteUncertainError, safe_exception_record +from isaac_autodata_core.autonomous.task_motion import ( + IDENTITY_MATRIX4, + AttachIntentSegment, + BarrierSegment, + CartesianTrajectorySegment, + ConcurrentGroupSegment, + DetachIntentSegment, + ExecutionEvent, + ExecutionEventType, + ExecutionOutcome, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + JointTrajectorySegment, + PlanSegment, + RobotStateSnapshot, + SceneObjectSnapshot, + SceneSnapshot, + TaskMotionPlan, + WaitSegment, + make_stable_id, + matrix4_error, + matrix4_inverse, + matrix4_multiply, +) + +__all__ = [ + "AttachIntentSegment", + "BarrierSegment", + "CartesianTrajectorySegment", + "ConcurrentGroupSegment", + "DetachIntentSegment", + "DatasetArtifact", + "ExecutionEvent", + "ExecutionEventType", + "ExecutionOutcome", + "GoalPredicate", + "IDENTITY_MATRIX4", + "GripperCommandMode", + "GripperCommandSegment", + "JointTrajectorySegment", + "DatasetCommitUncertainError", + "OutputTransaction", + "PlanSegment", + "RunLogWriteUncertainError", + "RunLogWriter", + "RecordingTargets", + "RequestDirectoryAnchor", + "RobotStateSnapshot", + "SceneObjectSnapshot", + "SceneSnapshot", + "TaskMotionPlan", + "WaitSegment", + "make_stable_id", + "matrix4_error", + "matrix4_inverse", + "matrix4_multiply", + "safe_exception_record", +] diff --git a/isaac_autodata_core/autonomous/attempt_generation.py b/isaac_autodata_core/autonomous/attempt_generation.py new file mode 100644 index 0000000..83d7ac7 --- /dev/null +++ b/isaac_autodata_core/autonomous/attempt_generation.py @@ -0,0 +1,514 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Source-demo-free attempt lifecycle independent of simulator and planner backends.""" + +from __future__ import annotations + +import asyncio +import time +from collections.abc import Mapping +from contextlib import suppress +from dataclasses import dataclass, field +from enum import StrEnum +from typing import Any, Protocol + +from isaac_autodata_core.autonomous.run_log import ( + RunLogWriter, + RunLogWriteUncertainError, + normalize_run_record, + safe_exception_record, +) +from isaac_autodata_core.autonomous.task_motion import ( + ExecutionEvent, + GoalPredicate, + JsonValue, + SceneSnapshot, + TaskMotionPlan, + make_stable_id, +) + +MAX_INLINE_PLAN_RECORD_BYTES = 512_000 +MAX_RESET_EVIDENCE_RECORD_BYTES = 64_000 + + +class FailureStage(StrEnum): + RESET = "reset" + SNAPSHOT = "snapshot" + PLANNING = "planning" + LOWERING = "lowering" + EXECUTION = "execution" + VERIFICATION = "verification" + RECORDING = "recording" + + +class AttemptGenerationError(RuntimeError): + """Expected, classified failure in one autonomous generation attempt.""" + + def __init__(self, stage: FailureStage, code: str, message: str, *, recoverable: bool = True) -> None: + self.stage = stage + self.code = code + self.recoverable = recoverable + super().__init__(message) + + +@dataclass(frozen=True) +class ExecutionResult: + """Final, evidence-based executor result for one task-motion plan.""" + + success: bool + events: tuple[ExecutionEvent, ...] + final_observation: Mapping[str, JsonValue] = field(default_factory=dict) + failure_stage: FailureStage | None = None + failure_code: str | None = None + failure_message: str | None = None + recoverable: bool = True + + def __post_init__(self) -> None: + if self.success and self.failure_code is not None: + raise ValueError("successful execution cannot have failure_code") + if self.success and self.failure_stage is not None: + raise ValueError("successful execution cannot have failure_stage") + if not self.success and not self.failure_code: + raise ValueError("failed execution requires failure_code") + if type(self.recoverable) is not bool: + raise ValueError("execution recoverable must be a boolean") + + +@dataclass(frozen=True) +class AttemptRequest: + """Resolved immutable inputs for exactly one environment attempt.""" + + request_digest: str + attempt_index: int + seed: int + env_id: int + goal: tuple[GoalPredicate, ...] + keep_failed: bool + expected_plan_backend: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.request_digest, str) or not self.request_digest.strip(): + raise ValueError("request_digest must be a non-empty string") + for field_name in ("attempt_index", "seed", "env_id"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, int) or value < 0: + raise ValueError(f"{field_name} must be a non-negative integer") + if not self.goal or any(not isinstance(item, GoalPredicate) for item in self.goal): + raise ValueError("goal must contain GoalPredicate instances") + if type(self.keep_failed) is not bool: + raise ValueError("keep_failed must be a boolean") + if self.expected_plan_backend is not None and ( + not isinstance(self.expected_plan_backend, str) or not self.expected_plan_backend.strip() + ): + raise ValueError("expected_plan_backend must be null or a non-empty string") + + @property + def attempt_id(self) -> str: + return make_stable_id("attempt", self.request_digest, self.attempt_index, self.seed, self.env_id) + + +@dataclass(frozen=True) +class AttemptResult: + """Complete result of one source-demo-free attempt, including no-action failures.""" + + attempt_id: str + success: bool + initial_state: Mapping[str, Any] + snapshot: SceneSnapshot | None + plan: TaskMotionPlan | None + execution: ExecutionResult | None + failure_stage: FailureStage | None = None + failure_code: str | None = None + failure_message: str | None = None + recoverable: bool = True + + +class AttemptRuntime(Protocol): + """Simulator/recorder operations required by the autonomous lifecycle.""" + + async def reset_attempt(self, env_id: int) -> Mapping[str, Any]: + pass + + def capture_scene_snapshot(self, env_id: int, *, snapshot_id: str) -> SceneSnapshot: + pass + + async def finish_attempt(self, env_id: int, *, success: bool, keep_failed: bool) -> None: + pass + + +class EpisodePlanner(Protocol): + """Planner backend boundary. Native planner types stay behind this interface.""" + + def plan(self, request: AttemptRequest, snapshot: SceneSnapshot) -> TaskMotionPlan: + pass + + def close(self) -> None: + pass + + +class EpisodeExecutor(Protocol): + """Execution boundary for one typed task-motion plan.""" + + async def execute(self, request: AttemptRequest, plan: TaskMotionPlan) -> ExecutionResult: + pass + + +class AttemptGenerator: + """Run bounded, source-demo-free planning attempts with durable run_log.""" + + def __init__( + self, + runtime: AttemptRuntime, + planner: EpisodePlanner, + executor: EpisodeExecutor, + *, + run_log_writer: RunLogWriter | None = None, + include_debug_tracebacks: bool = False, + ) -> None: + self.runtime = runtime + self.planner = planner + self.executor = executor + self.run_log_writer = run_log_writer + self.include_debug_tracebacks = include_debug_tracebacks + + async def generate_attempt(self, request: AttemptRequest) -> AttemptResult: + """Run one attempt and finalize recorder state even when the task is cancelled.""" + + try: + return await self._generate_attempt(request) + except BaseException as exc: + if isinstance(exc, Exception): + raise + await _finish_attempt_before_reraise(self.runtime, request) + raise + + async def _generate_attempt(self, request: AttemptRequest) -> AttemptResult: + """Reset, snapshot, plan, execute, verify, record, and account for one attempt.""" + + initial_state: Mapping[str, Any] = {} + snapshot: SceneSnapshot | None = None + plan: TaskMotionPlan | None = None + execution: ExecutionResult | None = None + failure: AttemptGenerationError | None = None + started_at = time.time() + + try: + try: + initial_state = await self.runtime.reset_attempt(request.env_id) + except Exception as exc: + raise _classify(exc, FailureStage.RESET, "reset_failed") from exc + + try: + snapshot_id = make_stable_id("snapshot", request.attempt_id, request.seed) + snapshot = self.runtime.capture_scene_snapshot(request.env_id, snapshot_id=snapshot_id) + except Exception as exc: + raise _classify(exc, FailureStage.SNAPSHOT, "snapshot_failed") from exc + + try: + plan = self.planner.plan(request, snapshot) + except Exception as exc: + raise _classify(exc, FailureStage.PLANNING, "planning_failed") from exc + if plan.request_digest != request.request_digest: + raise AttemptGenerationError( + FailureStage.LOWERING, + "request_digest_mismatch", + "planner returned a plan for a different request digest", + recoverable=False, + ) + if plan.snapshot_digest != snapshot.digest: + raise AttemptGenerationError( + FailureStage.LOWERING, + "snapshot_digest_mismatch", + "planner returned a plan for a different scene snapshot", + recoverable=False, + ) + if plan.seed != request.seed: + raise AttemptGenerationError( + FailureStage.LOWERING, + "seed_mismatch", + "planner returned a plan generated with a different attempt seed", + recoverable=False, + ) + if plan.goal != request.goal: + raise AttemptGenerationError( + FailureStage.LOWERING, + "goal_mismatch", + "planner returned a plan for different goal predicates", + recoverable=False, + ) + if request.expected_plan_backend is not None and plan.backend != request.expected_plan_backend: + raise AttemptGenerationError( + FailureStage.LOWERING, + "plan_backend_mismatch", + "planner returned a plan from an unexpected backend", + recoverable=False, + ) + + try: + execution = await self.executor.execute(request, plan) + except Exception as exc: + raise _classify(exc, FailureStage.EXECUTION, "execution_failed") from exc + if not execution.success: + failure = AttemptGenerationError( + execution.failure_stage or FailureStage.VERIFICATION, + execution.failure_code or "task_not_verified", + execution.failure_message or "final task postconditions were not verified", + recoverable=execution.recoverable, + ) + except AttemptGenerationError as exc: + failure = exc + + success = failure is None and execution is not None and execution.success + result = _attempt_result( + request, + success=success, + initial_state=initial_state, + snapshot=snapshot, + plan=plan, + execution=execution, + failure=failure, + ) + run_log_prepared = False + run_log_write_uncertainty: RunLogWriteUncertainError | None = None + try: + self._write_attempt_prepared( + request, + result, + started_at=started_at, + prepared_at=time.time(), + ) + run_log_prepared = self.run_log_writer is not None + except RunLogWriteUncertainError as exc: + run_log_write_uncertainty = exc + failure = AttemptGenerationError( + FailureStage.RECORDING, + "run_log_write_uncertain", + str(exc), + recoverable=False, + ) + success = False + except Exception as exc: + safe = safe_exception_record(exc) + failure = AttemptGenerationError( + FailureStage.RECORDING, + "run_log_prepare_failed", + safe["message"] or safe["exception_type"], + recoverable=False, + ) + success = False + + try: + await self.runtime.finish_attempt(request.env_id, success=success, keep_failed=request.keep_failed) + except Exception as exc: + safe = safe_exception_record(exc) + failure = AttemptGenerationError( + FailureStage.RECORDING, + "dataset_finalize_failed", + safe["message"] or safe["exception_type"], + recoverable=False, + ) + success = False + + if run_log_write_uncertainty is not None: + raise run_log_write_uncertainty + + result = _attempt_result( + request, + success=success, + initial_state=initial_state, + snapshot=snapshot, + plan=plan, + execution=execution, + failure=failure, + ) + if run_log_prepared: + try: + self._write_attempt_committed(request, result, completed_at=time.time()) + except RunLogWriteUncertainError: + raise + except Exception as exc: + safe = safe_exception_record(exc) + failure = AttemptGenerationError( + FailureStage.RECORDING, + "run_log_commit_failed", + safe["message"] or safe["exception_type"], + recoverable=False, + ) + result = _attempt_result( + request, + success=False, + initial_state=initial_state, + snapshot=snapshot, + plan=plan, + execution=execution, + failure=failure, + ) + return result + + def close(self) -> None: + """Release planner-owned GPU and service resources.""" + + self.planner.close() + + def _write_attempt_prepared( + self, + request: AttemptRequest, + result: AttemptResult, + *, + started_at: float, + prepared_at: float, + ) -> None: + if self.run_log_writer is None: + return + normalized_reset_record = normalize_run_record( + {"reset_evidence": result.initial_state}, + max_serialized_bytes=MAX_RESET_EVIDENCE_RECORD_BYTES, + ) + assert isinstance(normalized_reset_record, dict) + reset_evidence = normalized_reset_record["reset_evidence"] + if not isinstance(reset_evidence, dict): + raise ValueError("attempt reset evidence must be a JSON-compatible mapping") + self.run_log_writer.append({ + "attempt_id": result.attempt_id, + "attempt_index": request.attempt_index, + "prepared_at_unix_s": prepared_at, + "record_type": "attempt_prepared", + "env_id": request.env_id, + "events": [] if result.execution is None else [event.to_dict() for event in result.execution.events], + "failure": ( + None + if result.failure_code is None + else { + "code": result.failure_code, + "message": result.failure_message, + "recoverable": result.recoverable, + "stage": result.failure_stage.value if result.failure_stage is not None else None, + } + ), + "final_observation": {} if result.execution is None else dict(result.execution.final_observation), + "goal": [predicate.to_dict() for predicate in request.goal], + "plan": _plan_record(result.plan), + "request_digest": request.request_digest, + "reset_evidence": reset_evidence, + "seed": request.seed, + "snapshot": None if result.snapshot is None else result.snapshot.to_dict(), + "started_at_unix_s": started_at, + "success": result.success, + }) + + def _write_attempt_committed( + self, + request: AttemptRequest, + result: AttemptResult, + *, + completed_at: float, + ) -> None: + assert self.run_log_writer is not None + self.run_log_writer.append({ + "attempt_id": result.attempt_id, + "attempt_index": request.attempt_index, + "completed_at_unix_s": completed_at, + "failure_code": result.failure_code, + "record_type": "attempt_recorded", + "request_digest": request.request_digest, + "success": result.success, + }) + + +async def _finish_attempt_before_reraise(runtime: AttemptRuntime, request: AttemptRequest) -> None: + """Retain and await one finalizer despite repeated cancellation of the caller.""" + + try: + asyncio.get_running_loop() + except RuntimeError: + # GUI generation is driven inline because Kit owns the main-thread event loop. The reviewed + # live finalizer does not suspend, so it can still preserve recorder state before an + # operator interrupt propagates. A finalization failure must not hide that interrupt. + with suppress(BaseException): + await runtime.finish_attempt( + request.env_id, + success=False, + keep_failed=request.keep_failed, + ) + return + + finalizer = asyncio.create_task( + runtime.finish_attempt( + request.env_id, + success=False, + keep_failed=request.keep_failed, + ) + ) + while not finalizer.done(): + try: + await asyncio.shield(finalizer) + except asyncio.CancelledError: + continue + except BaseException: + return + try: + finalizer.result() + except BaseException: + # Preserve the original operator cancellation/interrupt. Runtime finalization is + # idempotent and the process-level terminal run log records interrupted execution. + return + + +def _attempt_result( + request: AttemptRequest, + *, + success: bool, + initial_state: Mapping[str, Any], + snapshot: SceneSnapshot | None, + plan: TaskMotionPlan | None, + execution: ExecutionResult | None, + failure: AttemptGenerationError | None, +) -> AttemptResult: + return AttemptResult( + attempt_id=request.attempt_id, + success=success, + initial_state=initial_state, + snapshot=snapshot, + plan=plan, + execution=execution, + failure_stage=None if failure is None else failure.stage, + failure_code=None if failure is None else failure.code, + failure_message=None if failure is None else str(failure), + recoverable=True if failure is None else failure.recoverable, + ) + + +def _plan_record(plan: TaskMotionPlan | None) -> Mapping[str, Any] | None: + if plan is None: + return None + canonical = plan.canonical_json() + summary: dict[str, Any] = { + "backend": {"name": plan.backend, "version": plan.backend_version}, + "digest": plan.digest, + "plan_id": plan.plan_id, + "segment_count": len(plan.segments), + "serialized_bytes": len(canonical.encode("utf-8")), + } + if summary["serialized_bytes"] <= MAX_INLINE_PLAN_RECORD_BYTES: + summary["representation"] = "inline" + summary["value"] = plan.to_dict() + else: + summary["representation"] = "digest_only" + summary["segments"] = [ + { + "kind": segment.kind.value, + "segment_id": segment.segment_id, + } + for segment in plan.segments + ] + return summary + + +def _classify(exc: Exception, default_stage: FailureStage, default_code: str) -> AttemptGenerationError: + if isinstance(exc, AttemptGenerationError): + return exc + safe = safe_exception_record(exc) + return AttemptGenerationError(default_stage, default_code, safe["message"] or safe["exception_type"]) diff --git a/isaac_autodata_core/autonomous/dataset_generation.py b/isaac_autodata_core/autonomous/dataset_generation.py new file mode 100644 index 0000000..b406ad5 --- /dev/null +++ b/isaac_autodata_core/autonomous/dataset_generation.py @@ -0,0 +1,165 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Bounded dataset-level run loop for source-free autonomous generation.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from isaac_autodata_core.autonomous.attempt_generation import AttemptGenerator, AttemptRequest, AttemptResult +from isaac_autodata_core.autonomous.task_motion import GoalPredicate + +MAX_ATTEMPT_SEED = 2**63 - 1 + + +@dataclass(frozen=True) +class DatasetGenerationRequest: + """Immutable dataset-level controls shared by all generation attempts.""" + + request_digest: str + goal: tuple[GoalPredicate, ...] + successful_episodes: int + max_attempts: int + base_seed: int + num_envs: int = 1 + keep_failed: bool = False + expected_plan_backend: str | None = None + + def __post_init__(self) -> None: + if not isinstance(self.request_digest, str) or not self.request_digest.strip(): + raise ValueError("request_digest must be a non-empty string") + if not self.goal or any(not isinstance(item, GoalPredicate) for item in self.goal): + raise ValueError("goal must contain GoalPredicate instances") + for field_name in ("successful_episodes", "max_attempts", "base_seed", "num_envs"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, int): + raise ValueError(f"{field_name} must be an integer") + if self.successful_episodes <= 0: + raise ValueError("successful_episodes must be positive") + if self.max_attempts < self.successful_episodes: + raise ValueError("max_attempts must be at least successful_episodes") + if not 0 <= self.base_seed <= MAX_ATTEMPT_SEED: + raise ValueError(f"base_seed must be in [0, {MAX_ATTEMPT_SEED}]") + if self.num_envs <= 0: + raise ValueError("num_envs must be positive") + if type(self.keep_failed) is not bool: + raise ValueError("keep_failed must be a boolean") + if self.expected_plan_backend is not None and ( + not isinstance(self.expected_plan_backend, str) or not self.expected_plan_backend.strip() + ): + raise ValueError("expected_plan_backend must be null or a non-empty string") + + +@dataclass(frozen=True) +class DatasetGenerationSummary: + """Terminal counters and reason for one bounded generation run.""" + + request_digest: str + requested_successful_episodes: int + attempts: int + successes: int + failures: int + stop_reason: str + target_reached: bool + last_result: AttemptResult | None + + def __post_init__(self) -> None: + if self.successes + self.failures != self.attempts: + raise ValueError("successes and failures must sum to attempts") + + def to_dict(self) -> dict[str, int | str | bool | None]: + """Return a compact JSON-compatible run summary.""" + + return { + "attempts": self.attempts, + "failures": self.failures, + "last_attempt_id": None if self.last_result is None else self.last_result.attempt_id, + "request_digest": self.request_digest, + "requested_successful_episodes": self.requested_successful_episodes, + "stop_reason": self.stop_reason, + "successes": self.successes, + "target_reached": self.target_reached, + } + + +async def generate_dataset( + generator: AttemptGenerator, + request: DatasetGenerationRequest, + *, + close: bool = True, +) -> DatasetGenerationSummary: + """Run deterministic attempts until the requested target or a hard stop is reached. + + Failed attempts always count toward ``max_attempts``. Only attempts that pass execution and + task-success validation count toward ``successful_episodes``. A classified non-recoverable + failure ends the run immediately. Planner resources are closed in a ``finally`` block by + default. + + Args: + generator: Configured source-free attempt orchestrator. + request: Dataset-level generation controls. + close: Whether to close planner-owned resources before returning or propagating. + """ + + attempts = 0 + successes = 0 + failures = 0 + last_result: AttemptResult | None = None + stop_reason = "max_attempts" + target_reached = False + + try: + while attempts < request.max_attempts: + if successes >= request.successful_episodes: + target_reached = True + stop_reason = "requested_successes" + break + + attempt_request = AttemptRequest( + request_digest=request.request_digest, + attempt_index=attempts, + seed=_attempt_seed(request.base_seed, attempts), + env_id=attempts % request.num_envs, + goal=request.goal, + keep_failed=request.keep_failed, + expected_plan_backend=request.expected_plan_backend, + ) + last_result = await generator.generate_attempt(attempt_request) + attempts += 1 + if last_result.success: + successes += 1 + else: + failures += 1 + if not last_result.recoverable: + stop_reason = "unrecoverable_failure" + break + else: + stop_reason = "max_attempts" + + if not target_reached: + if successes >= request.successful_episodes: + target_reached = True + stop_reason = "requested_successes" + + return DatasetGenerationSummary( + request_digest=request.request_digest, + requested_successful_episodes=request.successful_episodes, + attempts=attempts, + successes=successes, + failures=failures, + stop_reason=stop_reason, + target_reached=target_reached, + last_result=last_result, + ) + finally: + if close: + generator.close() + + +def _attempt_seed(base_seed: int, attempt_index: int) -> int: + """Derive a deterministic signed-int64-compatible attempt seed without overflow.""" + + return (base_seed + attempt_index) % (MAX_ATTEMPT_SEED + 1) diff --git a/isaac_autodata_core/autonomous/dense_trace.py b/isaac_autodata_core/autonomous/dense_trace.py new file mode 100644 index 0000000..efb9ac8 --- /dev/null +++ b/isaac_autodata_core/autonomous/dense_trace.py @@ -0,0 +1,219 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Lower aligned backend samples into the typed task-motion plan IR.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + DetachIntentSegment, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + Matrix4, + TaskMotionPlan, + TaskMotionSegment, + make_stable_id, + matrix4, +) + + +@dataclass(frozen=True) +class DenseAttachmentEvent: + """Symbolic attachment transition aligned to one dense sample index.""" + + sample_index: int + operation: str + object_name: str + verifier: str = "contact_and_relative_motion_v1" + + def __post_init__(self) -> None: + if isinstance(self.sample_index, bool) or not isinstance(self.sample_index, int) or self.sample_index < 0: + raise ValueError("sample_index must be a non-negative integer") + if self.operation not in ("attach", "detach"): + raise ValueError("attachment operation must be 'attach' or 'detach'") + if not self.object_name: + raise ValueError("attachment object_name must not be empty") + if not self.verifier: + raise ValueError("attachment verifier must not be empty") + + +@dataclass(frozen=True) +class DensePlanTrace: + """Backend-neutral aligned poses, joint seeds, gripper values, and symbolic events.""" + + eef_name: str + frame: str + poses: tuple[Matrix4, ...] + gripper_values: tuple[float, ...] + step_dt_s: float + gripper_settle_steps: int = 1 + joint_names: tuple[str, ...] = () + joint_positions: tuple[tuple[float, ...], ...] = () + attachment_events: tuple[DenseAttachmentEvent, ...] = () + + def __post_init__(self) -> None: + if not self.eef_name or not self.frame: + raise ValueError("eef_name and frame must not be empty") + if not self.poses: + raise ValueError("dense plan trace must contain at least one pose") + object.__setattr__( + self, + "poses", + tuple(matrix4(pose, f"poses[{index}]") for index, pose in enumerate(self.poses)), + ) + if len(self.gripper_values) != len(self.poses): + raise ValueError("gripper_values must align one-to-one with poses") + values = tuple(float(value) for value in self.gripper_values) + if any(value != value or value in (float("inf"), float("-inf")) for value in values): + raise ValueError("gripper_values must be finite") + object.__setattr__(self, "gripper_values", values) + if self.step_dt_s <= 0: + raise ValueError("step_dt_s must be positive") + if ( + isinstance(self.gripper_settle_steps, bool) + or not isinstance(self.gripper_settle_steps, int) + or not 1 <= self.gripper_settle_steps <= 10_000 + ): + raise ValueError("gripper_settle_steps must be an integer in [1, 10000]") + if self.joint_positions: + if not self.joint_names or len(set(self.joint_names)) != len(self.joint_names): + raise ValueError("joint_names must be non-empty and unique when joint positions are present") + if len(self.joint_positions) != len(self.poses): + raise ValueError("joint_positions must align one-to-one with poses") + if any(len(position) != len(self.joint_names) for position in self.joint_positions): + raise ValueError("joint position widths must match joint_names") + elif self.joint_names: + raise ValueError("joint_names must be empty when joint_positions are absent") + if any(event.sample_index >= len(self.poses) for event in self.attachment_events): + raise ValueError("attachment event sample index is outside the dense trace") + if any(event.sample_index == 0 for event in self.attachment_events): + raise ValueError("attachment events require a preceding collision-checked dense sample") + event_keys = [(event.sample_index, event.operation, event.object_name) for event in self.attachment_events] + if len(event_keys) != len(set(event_keys)): + raise ValueError("dense trace attachment events must be unique") + split_indices = { + index + for index in range(1, len(self.gripper_values)) + if abs(self.gripper_values[index] - self.gripper_values[index - 1]) > 1e-6 + } + split_indices.update(event.sample_index for event in self.attachment_events if event.sample_index > 0) + for index in sorted(split_indices): + if self.poses[index] != self.poses[index - 1]: + raise ValueError( + f"dense trace split at sample {index} must duplicate the preceding pose before interaction" + ) + if self.joint_positions and self.joint_positions[index] != self.joint_positions[index - 1]: + raise ValueError( + f"dense trace split at sample {index} must duplicate the preceding joint seed before interaction" + ) + + +def task_motion_plan_from_dense_trace( + trace: DensePlanTrace, + *, + request_digest: str, + snapshot_digest: str, + backend: str, + backend_version: str, + seed: int, + goal: tuple[GoalPredicate, ...], + metadata: dict | None = None, +) -> TaskMotionPlan: + """Split a dense trace at gripper/attachment changes and build a sequential typed plan.""" + + split_indices = {0, len(trace.poses)} + for index in range(1, len(trace.gripper_values)): + if abs(trace.gripper_values[index] - trace.gripper_values[index - 1]) > 1e-6: + split_indices.add(index) + for event in trace.attachment_events: + split_indices.add(event.sample_index) + boundaries = sorted(split_indices) + events_by_index: dict[int, list[DenseAttachmentEvent]] = {} + for event in trace.attachment_events: + events_by_index.setdefault(event.sample_index, []).append(event) + + segments: list[TaskMotionSegment] = [] + previous_id: str | None = None + + def append(segment: TaskMotionSegment) -> None: + nonlocal previous_id + segments.append(segment) + previous_id = segment.segment_id + + previous_gripper: float | None = None + for start, end in zip(boundaries, boundaries[1:]): + gripper = trace.gripper_values[start] + if previous_gripper is None or abs(gripper - previous_gripper) > 1e-6: + command, value = _gripper_command(gripper) + segment_id = make_stable_id("gripper", request_digest, start, command.value, value) + append( + GripperCommandSegment( + segment_id=segment_id, + depends_on=(() if previous_id is None else (previous_id,)), + eef_name=trace.eef_name, + command=command, + value=value, + settle_steps=trace.gripper_settle_steps, + ) + ) + previous_gripper = gripper + + for event in sorted(events_by_index.get(start, ()), key=lambda item: (item.operation, item.object_name)): + segment_id = make_stable_id("attachment", request_digest, start, event.operation, event.object_name) + event_kwargs = { + "segment_id": segment_id, + "depends_on": () if previous_id is None else (previous_id,), + "eef_name": trace.eef_name, + "object_name": event.object_name, + "verifier": event.verifier, + } + if event.operation == "attach": + append(AttachIntentSegment(**event_kwargs)) + else: + append(DetachIntentSegment(**event_kwargs)) + + if end <= start: + continue + segment_id = make_stable_id("cartesian", request_digest, start, end) + joint_seeds = trace.joint_positions[start:end] if trace.joint_positions else () + append( + CartesianTrajectorySegment( + segment_id=segment_id, + depends_on=(() if previous_id is None else (previous_id,)), + duration_s=(end - start) * trace.step_dt_s, + eef_name=trace.eef_name, + frame=trace.frame, + poses=trace.poses[start:end], + joint_seed_names=trace.joint_names, + joint_seeds=joint_seeds, + metadata={"sample_end_exclusive": end, "sample_start": start, "step_dt_s": trace.step_dt_s}, + ) + ) + + plan_id = make_stable_id("plan", request_digest, snapshot_digest, backend, seed) + return TaskMotionPlan( + plan_id=plan_id, + request_digest=request_digest, + snapshot_digest=snapshot_digest, + backend=backend, + backend_version=backend_version, + seed=seed, + segments=tuple(segments), + goal=goal, + metadata={} if metadata is None else metadata, + ) + + +def _gripper_command(value: float) -> tuple[GripperCommandMode, float | None]: + if value >= 1.0 - 1e-6: + return GripperCommandMode.OPEN, None + if value <= -1.0 + 1e-6: + return GripperCommandMode.CLOSE, None + return GripperCommandMode.POSITION, value diff --git a/isaac_autodata_core/autonomous/hdf5_validation.py b/isaac_autodata_core/autonomous/hdf5_validation.py new file mode 100644 index 0000000..216d1ef --- /dev/null +++ b/isaac_autodata_core/autonomous/hdf5_validation.py @@ -0,0 +1,197 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Bounded structural and numeric validation for recorded autonomous HDF5 datasets.""" + +from __future__ import annotations + +import itertools +import os +import re +from collections.abc import Iterator +from numbers import Integral +from typing import Any + +_HDF5_VALIDATION_CHUNK_BYTES = 8 << 20 + + +def hdf5_episode_count(descriptor: int, *, kind: str) -> int: + """Validate a held HDF5 dataset and return its episode count. + + Args: + descriptor: Readable descriptor for the exact staged dataset inode. + kind: Dataset role: ``successful``, ``failed``, or ``filesystem probe``. + + Returns: + Number of validated episode groups. + """ + + signature = os.pread(descriptor, 8, 0) + if signature != b"\x89HDF\r\n\x1a\n": + raise ValueError(f"staged {kind} dataset does not have an HDF5 signature") + try: + import h5py + except ImportError as exc: + raise RuntimeError("h5py is required to validate staged autonomous datasets") from exc + + duplicate = os.dup(descriptor) + with os.fdopen(duplicate, "rb", closefd=True) as stream: + with h5py.File(stream, "r") as dataset: + data = dataset.get("data") + if not isinstance(data, h5py.Group): + raise ValueError(f"staged {kind} dataset has no HDF5 'data' group") + if kind == "filesystem probe": + return len(data) + format_version = dataset.attrs.get("format_version") + if isinstance(format_version, bool) or not isinstance(format_version, Integral) or format_version != 1: + raise ValueError(f"staged {kind} dataset has unsupported or missing format_version") + episode_names = list(data) + episode_indices: list[int] = [] + expected_success = kind == "successful" + for episode_name in episode_names: + match = re.fullmatch(r"demo_(\d+)", episode_name) + if match is None: + raise ValueError(f"staged {kind} dataset has invalid episode name {episode_name!r}") + episode_indices.append(int(match.group(1))) + episode = data[episode_name] + if not isinstance(episode, h5py.Group): + raise ValueError(f"staged {kind} dataset episode {episode_name!r} is not a group") + _validate_hdf5_episode(episode, episode_name, expected_success=expected_success, h5py=h5py) + if sorted(episode_indices) != list(range(len(episode_indices))): + raise ValueError(f"staged {kind} dataset episode indices must be contiguous from zero") + return len(episode_names) + + +def _validate_hdf5_episode(episode: Any, episode_name: str, *, expected_success: bool, h5py: Any) -> None: + import numpy as np + + initial_state = episode.get("initial_state") + if not isinstance(initial_state, h5py.Group): + raise ValueError(f"staged dataset episode {episode_name!r} has no 'initial_state' group") + num_samples = episode.attrs.get("num_samples") + minimum_samples = 1 if expected_success else 0 + if isinstance(num_samples, bool) or not isinstance(num_samples, Integral) or num_samples < minimum_samples: + raise ValueError(f"staged dataset episode {episode_name!r} has invalid num_samples") + success = episode.attrs.get("success") + if not isinstance(success, (bool, np.bool_)) or bool(success) is not expected_success: + raise ValueError(f"staged dataset episode {episode_name!r} has inconsistent success metadata") + + # A failed attempt can be retained before its first task step. Such an episode has a + # reset-time initial state and num_samples=0, but no task-stream groups or action datasets. + require_task_stream = expected_success or num_samples > 0 + task_groups: dict[str, Any] = {} + for name in ("obs", "states"): + value = episode.get(name) + if value is None and not require_task_stream: + continue + if not isinstance(value, h5py.Group): + raise ValueError(f"staged dataset episode {episode_name!r} has no {name!r} group") + task_groups[name] = value + task_datasets: dict[str, Any] = {} + for name in ("actions", "processed_actions"): + value = episode.get(name) + if value is None and not require_task_stream: + continue + if not isinstance(value, h5py.Dataset) or len(value.shape) != 2 or value.shape[0] != num_samples: + raise ValueError(f"staged dataset episode {episode_name!r} {name!r} must be rank-2 with num_samples rows") + if value.shape[1] < 1: + raise ValueError(f"staged dataset episode {episode_name!r} {name!r} must have a nonempty feature axis") + task_datasets[name] = value + for name, value in task_datasets.items(): + _require_finite_hdf5_dataset(value, episode_name, name) + _validate_hdf5_dataset_group( + initial_state, + episode_name, + "initial_state", + h5py=h5py, + require_datasets=expected_success, + require_nonempty_datasets=expected_success, + ) + for group_name, group in task_groups.items(): + _validate_hdf5_dataset_group( + group, + episode_name, + group_name, + h5py=h5py, + expected_rows=num_samples, + require_datasets=require_task_stream, + ) + + +def _validate_hdf5_dataset_group( + group: Any, + episode_name: str, + group_name: str, + *, + h5py: Any, + expected_rows: int | None = None, + require_datasets: bool, + require_nonempty_datasets: bool = False, +) -> None: + datasets: list[tuple[str, Any]] = [] + + def collect(name: str, value: Any) -> None: + if isinstance(value, h5py.Dataset): + datasets.append((name, value)) + + group.visititems(collect) + if require_datasets and not datasets: + raise ValueError(f"staged dataset episode {episode_name!r} {group_name!r} group has no datasets") + for dataset_name, value in datasets: + qualified_name = f"{group_name}/{dataset_name}" + if expected_rows is not None and (len(value.shape) < 1 or value.shape[0] != expected_rows): + raise ValueError( + f"staged dataset episode {episode_name!r} {qualified_name!r} must have num_samples leading rows" + ) + _require_finite_hdf5_dataset( + value, + episode_name, + qualified_name, + require_nonempty=require_nonempty_datasets, + ) + + +def _require_finite_hdf5_dataset( + dataset: Any, + episode_name: str, + dataset_name: str, + *, + require_nonempty: bool = False, +) -> None: + import numpy as np + + if not np.issubdtype(dataset.dtype, np.number): + raise ValueError(f"staged dataset episode {episode_name!r} {dataset_name!r} must be numeric") + if require_nonempty and dataset.size < 1: + raise ValueError(f"staged dataset episode {episode_name!r} {dataset_name!r} must not be empty") + for selection in _bounded_hdf5_selections(dataset.shape, dataset.dtype.itemsize): + values = dataset[()] if not selection else dataset[selection] + if not np.isfinite(values).all(): + raise ValueError(f"staged dataset episode {episode_name!r} {dataset_name!r} contains non-finite values") + + +def _bounded_hdf5_selections(shape: tuple[int, ...], itemsize: int) -> Iterator[tuple[slice, ...]]: + """Yield numeric hyperslabs no larger than the validation byte bound.""" + + if not shape: + yield () + return + if any(size == 0 for size in shape): + return + max_items = max(1, _HDF5_VALIDATION_CHUNK_BYTES // max(1, itemsize)) + block_shape = [1] * len(shape) + remaining_items = max_items + for axis in range(len(shape) - 1, -1, -1): + block_shape[axis] = min(shape[axis], remaining_items) + remaining_items = max(1, remaining_items // block_shape[axis]) + starts = (range(0, size, block_size) for size, block_size in zip(shape, block_shape)) + for offsets in itertools.product(*starts): + yield tuple( + slice(offset, min(offset + block_size, size)) + for offset, block_size, size in zip(offsets, block_shape, shape) + ) + + +__all__ = ["hdf5_episode_count"] diff --git a/isaac_autodata_core/autonomous/output_transaction.py b/isaac_autodata_core/autonomous/output_transaction.py new file mode 100644 index 0000000..59a177b --- /dev/null +++ b/isaac_autodata_core/autonomous/output_transaction.py @@ -0,0 +1,863 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Descriptor-safe reservation and publication of autonomous run outputs. + +The transaction is intentionally Linux/POSIX specific. Autonomous Isaac execution already runs in +the Linux runtime image, where directory descriptors and ``/proc/self/fd`` let the recorder write +through an identity that cannot be redirected by a later path or symlink replacement. +""" + +from __future__ import annotations + +import hashlib +import os +import secrets +import stat +from collections.abc import Mapping +from contextlib import suppress +from dataclasses import dataclass +from pathlib import Path, PurePath +from typing import Any + +from isaac_autodata_core.autonomous.hdf5_validation import hdf5_episode_count +from isaac_autodata_core.autonomous.run_log import RunLogWriter + +DEFAULT_MAX_DATASET_BYTES = 1 << 40 +DEFAULT_MAX_REQUEST_BYTES = 4 << 20 +_HASH_CHUNK_BYTES = 8 << 20 +_MAX_STAGING_NAME_ATTEMPTS = 32 + + +@dataclass(frozen=True) +class _ValidatedDataset: + staged_name: str + final_name: str + descriptor: int + device: int + inode: int + mtime_ns: int + artifact: DatasetArtifact + + +class RequestDirectoryAnchor: + """Held identity of the request file and its containing output root.""" + + def __init__(self, path: Path, directory_fd: int, request_fd: int) -> None: + self.path = path + self.directory = path.parent + self._directory_fd = directory_fd + self._request_fd = request_fd + self._directory_identity = _device_inode(os.fstat(directory_fd)) + self._request_identity = _device_inode(os.fstat(request_fd)) + self._closed = False + + @classmethod + def open( + cls, + request_path: str | Path, + *, + max_request_bytes: int = DEFAULT_MAX_REQUEST_BYTES, + ) -> RequestDirectoryAnchor: + """Open a regular request and its parent without following symlink components.""" + + if isinstance(max_request_bytes, bool) or not isinstance(max_request_bytes, int) or max_request_bytes < 1: + raise ValueError("max_request_bytes must be a positive integer") + path = _absolute_lexical_path(request_path) + directory_fd = _open_absolute_directory(path.parent) + request_fd: int | None = None + try: + request_fd = os.open(path.name, os.O_RDONLY | os.O_CLOEXEC | os.O_NOFOLLOW, dir_fd=directory_fd) + request_stat = os.fstat(request_fd) + if not stat.S_ISREG(request_stat.st_mode) or request_stat.st_nlink != 1: + raise ValueError("task request must be a regular file with exactly one link") + if request_stat.st_size <= 0 or request_stat.st_size > max_request_bytes: + raise ValueError( + f"task request size must be in [1, {max_request_bytes}] bytes, got {request_stat.st_size}" + ) + return cls(path, directory_fd, request_fd) + except Exception: + if request_fd is not None: + os.close(request_fd) + os.close(directory_fd) + raise + + def verify_current(self) -> None: + """Require the request pathname and parent to retain their held identities.""" + + if self._closed: + raise RuntimeError("request directory anchor is closed") + if _device_inode(os.fstat(self._directory_fd)) != self._directory_identity: + raise RuntimeError("attested request directory identity changed") + visible_directory_fd = _open_absolute_directory(self.directory) + try: + if _device_inode(os.fstat(visible_directory_fd)) != self._directory_identity: + raise RuntimeError("attested request directory path was renamed or replaced") + finally: + os.close(visible_directory_fd) + request_stat = os.fstat(self._request_fd) + path_stat = os.stat(self.path.name, dir_fd=self._directory_fd, follow_symlinks=False) + if not stat.S_ISREG(path_stat.st_mode) or path_stat.st_nlink != 1: + raise RuntimeError("attested request path is no longer a single-link regular file") + if _device_inode(request_stat) != self._request_identity or _device_inode(path_stat) != self._request_identity: + raise RuntimeError("attested request path identity changed during compilation") + + def duplicate_directory_fd(self) -> int: + """Return a caller-owned duplicate of the verified request-directory descriptor.""" + + self.verify_current() + return os.dup(self._directory_fd) + + def close(self) -> None: + """Close the held request and parent descriptors.""" + + if self._closed: + return + self._closed = True + os.close(self._request_fd) + os.close(self._directory_fd) + + +class DatasetCommitUncertainError(RuntimeError): + """Dataset links are durable but terminal run-log durability is unknown.""" + + +@dataclass(frozen=True) +class RecordingTargets: + """Recorder destination backed by a held private staging-directory descriptor. + + Attributes: + dataset_export_dir_path: Stable process-local path to the held staging directory. + dataset_filename: Dataset filename stem expected by Isaac Lab's recorder. + """ + + dataset_export_dir_path: str + dataset_filename: str + + +@dataclass(frozen=True) +class DatasetArtifact: + """Validated identity of one published HDF5 artifact.""" + + path: str + sha256: str + size_bytes: int + episode_count: int + kind: str + + def to_dict(self) -> dict[str, str | int]: + """Return a JSON-compatible artifact record.""" + + return { + "episode_count": self.episode_count, + "kind": self.kind, + "path": self.path, + "sha256": self.sha256, + "size_bytes": self.size_bytes, + } + + +class OutputTransaction: + """Own output directory identities from reservation through durable publication. + + Use :meth:`reserve` after child request attestation and before the Isaac recorder is built. + The caller must close the Arena environment before calling :meth:`publish`, because recorder + shutdown is the operation that flushes and closes the staged HDF5 file. + """ + + def __init__( + self, + *, + request_directory: Path, + dataset_path: Path, + failed_dataset_path: Path | None, + run_log_path: Path | None, + request_dir_fd: int, + dataset_parent_fd: int, + run_log_parent_fd: int | None, + run_log_fd: int | None, + staging_name: str, + staging_fd: int, + max_dataset_bytes: int, + ) -> None: + self.request_directory = request_directory + self.dataset_path = dataset_path + self.failed_dataset_path = failed_dataset_path + self.run_log_path = run_log_path + self._request_dir_fd = request_dir_fd + self._dataset_parent_fd = dataset_parent_fd + self._run_log_parent_fd = run_log_parent_fd + self._run_log_fd = run_log_fd + self._staging_name = staging_name + self._staging_fd = staging_fd + self._max_dataset_bytes = max_dataset_bytes + self._published = False + self._closed = False + + @classmethod + def reserve( + cls, + *, + request_directory: str | Path | None = None, + request_anchor: RequestDirectoryAnchor | None = None, + dataset_path: str | Path, + run_log_path: str | Path | None, + keep_failed: bool, + max_dataset_bytes: int = DEFAULT_MAX_DATASET_BYTES, + ) -> OutputTransaction: + """Reserve fresh outputs and a private dataset staging directory. + + Every path component is traversed relative to a held directory descriptor with + ``O_DIRECTORY | O_NOFOLLOW``. Missing output-parent components are created mode ``0700``. + The run_log file is created exclusively and retained open with ``O_APPEND``. + + Args: + request_directory: Canonical directory containing the attested request. + dataset_path: Canonical final ``.hdf5`` path below ``request_directory``. + run_log_path: Optional canonical final ``.jsonl`` path below the request directory. + keep_failed: Reserve publication support for the recorder's failed-episode dataset. + max_dataset_bytes: Maximum accepted size of each staged dataset. + """ + + if isinstance(max_dataset_bytes, bool) or not isinstance(max_dataset_bytes, int) or max_dataset_bytes < 1: + raise ValueError("max_dataset_bytes must be a positive integer") + if request_anchor is None and request_directory is None: + raise ValueError("request_directory or request_anchor is required") + if request_anchor is not None: + request_anchor.verify_current() + request_dir = request_anchor.directory + if request_directory is not None and _absolute_lexical_path(request_directory) != request_dir: + raise ValueError("request_directory does not match the held request anchor") + else: + assert request_directory is not None + request_dir = _absolute_lexical_path(request_directory) + dataset = _absolute_lexical_path(dataset_path) + run_log = None if run_log_path is None else _absolute_lexical_path(run_log_path) + if dataset.suffix.lower() != ".hdf5": + raise ValueError("dataset path must end in .hdf5") + if run_log is not None and run_log.suffix.lower() != ".jsonl": + raise ValueError("run_log path must end in .jsonl") + dataset_relative = _relative_output_path(dataset, request_dir, "dataset") + run_log_relative = None if run_log is None else _relative_output_path(run_log, request_dir, "run_log") + + request_fd = ( + request_anchor.duplicate_directory_fd() + if request_anchor is not None + else _open_absolute_directory(request_dir) + ) + dataset_parent_fd: int | None = None + run_log_parent_fd: int | None = None + run_log_fd: int | None = None + staging_name: str | None = None + staging_fd: int | None = None + try: + dataset_parent_fd = _open_relative_directory( + request_fd, + dataset_relative.parent.parts, + create=True, + ) + _require_missing_entry(dataset_relative.name, dataset_parent_fd, "dataset") + failed_dataset = dataset.with_name(f"{dataset.stem}_failed{dataset.suffix}") if keep_failed else None + if failed_dataset is not None: + _require_missing_entry(failed_dataset.name, dataset_parent_fd, "failed dataset") + + staging_name, staging_fd = _create_staging_directory(dataset_parent_fd) + _probe_output_filesystem(staging_fd, dataset_parent_fd) + + if run_log_relative is not None: + run_log_parent_fd = _open_relative_directory( + request_fd, + run_log_relative.parent.parts, + create=True, + ) + run_log_fd = _open_exclusive_run_log(run_log_relative.name, run_log_parent_fd) + _fsync_directory(run_log_parent_fd) + _require_visible_fd_identity(run_log_relative.name, run_log_parent_fd, run_log_fd) + + return cls( + request_directory=request_dir, + dataset_path=dataset, + failed_dataset_path=failed_dataset, + run_log_path=run_log, + request_dir_fd=request_fd, + dataset_parent_fd=dataset_parent_fd, + run_log_parent_fd=run_log_parent_fd, + run_log_fd=run_log_fd, + staging_name=staging_name, + staging_fd=staging_fd, + max_dataset_bytes=max_dataset_bytes, + ) + except Exception: + if run_log_fd is not None: + created_identity = _device_inode(os.fstat(run_log_fd)) + os.close(run_log_fd) + assert run_log_relative is not None + assert run_log_parent_fd is not None + try: + visible = os.stat( + run_log_relative.name, + dir_fd=run_log_parent_fd, + follow_symlinks=False, + ) + if stat.S_ISREG(visible.st_mode) and _device_inode(visible) == created_identity: + os.unlink(run_log_relative.name, dir_fd=run_log_parent_fd) + _fsync_directory(run_log_parent_fd) + except FileNotFoundError: + pass + if staging_fd is not None and staging_name is not None and dataset_parent_fd is not None: + with suppress(Exception): + _cleanup_private_staging(staging_fd, staging_name, dataset_parent_fd) + _close_distinct_fds_best_effort(run_log_parent_fd, dataset_parent_fd, request_fd) + raise + + @property + def recording_targets(self) -> RecordingTargets: + """Return the recorder override bound to the held staging directory.""" + + self._require_open() + return RecordingTargets( + dataset_export_dir_path=f"/proc/self/fd/{self._staging_fd}", + dataset_filename=self.dataset_path.stem, + ) + + def open_run_log_writer( + self, + writer_factory: Any = RunLogWriter, + ) -> RunLogWriter | Any | None: + """Create a writer on a duplicate of the exclusively reserved run_log descriptor. + + The returned writer owns its duplicate. The transaction retains the original descriptor so + the reserved inode stays held even if a caller closes the writer early. + """ + + self._require_open() + if self._run_log_fd is None or self.run_log_path is None: + return None + duplicate = os.dup(self._run_log_fd) + try: + return writer_factory(self.run_log_path, fd=duplicate, close_fd=True) + except Exception: + os.close(duplicate) + raise + + def publish( + self, + *, + run_log_writer: RunLogWriter | Any | None, + commit_record: Mapping[str, Any] | None, + require_failed_dataset: bool = False, + expected_successful_episodes: int | None = None, + expected_failed_episodes: int | None = None, + ) -> tuple[DatasetArtifact, ...]: + """Validate, atomically link, and durably commit staged artifacts. + + Final links are created with no replacement. If publication of a later artifact fails, links + created by this call are removed before the error is returned. The run_log commit record + is appended only after all final links and their parent directory have been fsynced. + """ + + self._require_open() + if self._published: + raise RuntimeError("output transaction has already been published") + terminal_record: dict[str, Any] | None = None + if commit_record is not None: + if run_log_writer is None: + raise ValueError("a run_log commit record requires a run_log writer") + terminal_record = dict(commit_record) + if "artifacts" in terminal_record: + raise ValueError("commit_record must not define the transaction-owned artifacts field") + specifications = [(self.dataset_path.name, self.dataset_path, "successful")] + if self.failed_dataset_path is not None: + failed_name = self.failed_dataset_path.name + if _entry_exists(failed_name, self._staging_fd): + specifications.append((failed_name, self.failed_dataset_path, "failed")) + elif require_failed_dataset: + raise FileNotFoundError("recorder did not produce the expected failed-episode dataset") + + expected_counts = { + "successful": expected_successful_episodes, + "failed": expected_failed_episodes, + } + validated: list[_ValidatedDataset] = [] + linked_items: list[_ValidatedDataset] = [] + try: + self._verify_output_parent_identities() + self._verify_run_log_identity() + for staged_name, final_path, kind in specifications: + validated.append( + self._validate_staged_dataset( + staged_name, + final_path, + kind, + expected_episode_count=expected_counts[kind], + ) + ) + artifacts = tuple(item.artifact for item in validated) + if terminal_record is not None: + terminal_record["artifacts"] = [artifact.to_dict() for artifact in artifacts] + for item in validated: + _link_fd_to_name(item.descriptor, self._dataset_parent_fd, item.final_name) + linked_items.append(item) + linked = os.stat(item.final_name, dir_fd=self._dataset_parent_fd, follow_symlinks=False) + if not stat.S_ISREG(linked.st_mode) or (linked.st_dev, linked.st_ino) != (item.device, item.inode): + raise RuntimeError("published dataset identity did not match the validated inode") + _fsync_directory(self._dataset_parent_fd) + for item in validated: + self._unlink_staged_dataset_inode(item) + _fsync_directory(self._staging_fd) + for item in validated: + self._verify_published_dataset(item) + self._verify_output_parent_identities() + self._verify_run_log_identity() + except Exception: + for item in reversed(linked_items): + try: + visible = os.stat(item.final_name, dir_fd=self._dataset_parent_fd, follow_symlinks=False) + except FileNotFoundError: + continue + if _device_inode(visible) == (item.device, item.inode): + os.unlink(item.final_name, dir_fd=self._dataset_parent_fd) + if linked_items: + _fsync_directory(self._dataset_parent_fd) + for item in validated: + with suppress(OSError): + os.close(item.descriptor) + validated.clear() + raise + + self._published = True + try: + if terminal_record is not None: + try: + assert run_log_writer is not None + run_log_writer.append(terminal_record) + self._verify_run_log_identity() + assert self._run_log_parent_fd is not None + _fsync_directory(self._run_log_parent_fd) + except Exception as exc: + raise DatasetCommitUncertainError( + "dataset links are durable, but terminal run_log append durability is unknown; " + "do not retry or delete outputs until run-log recovery is complete" + ) from exc + return artifacts + finally: + for item in validated: + with suppress(OSError): + os.close(item.descriptor) + + def close(self) -> None: + """Close held descriptors and remove the exact private staging directory.""" + + if self._closed: + return + self._closed = True + issues: list[BaseException] = [] + try: + _cleanup_private_staging(self._staging_fd, self._staging_name, self._dataset_parent_fd) + except BaseException as exc: + issues.append(exc) + if self._run_log_fd is not None: + try: + os.close(self._run_log_fd) + except OSError as exc: + issues.append(exc) + for descriptor in _distinct_fds( + self._run_log_parent_fd, + self._dataset_parent_fd, + self._request_dir_fd, + ): + try: + os.close(descriptor) + except OSError as exc: + issues.append(exc) + if issues: + raise OSError("; ".join(str(issue) for issue in issues)) + + def _validate_staged_dataset( + self, + staged_name: str, + final_path: Path, + kind: str, + *, + expected_episode_count: int | None, + ) -> _ValidatedDataset: + descriptor = os.open( + staged_name, + os.O_RDONLY | os.O_CLOEXEC | os.O_NOFOLLOW, + dir_fd=self._staging_fd, + ) + try: + before = os.fstat(descriptor) + if not stat.S_ISREG(before.st_mode): + raise ValueError(f"staged {kind} dataset is not a regular file") + if before.st_nlink != 1: + raise ValueError(f"staged {kind} dataset must have exactly one link before publication") + if before.st_size <= 0: + raise ValueError(f"staged {kind} dataset is empty") + if before.st_size > self._max_dataset_bytes: + raise ValueError( + f"staged {kind} dataset is {before.st_size} bytes; maximum is {self._max_dataset_bytes}" + ) + os.fchmod(descriptor, 0o600) + os.fsync(descriptor) + digest = _sha256_fd(descriptor, expected_size=before.st_size) + episode_count = hdf5_episode_count(descriptor, kind=kind) + if expected_episode_count is not None and episode_count != expected_episode_count: + raise ValueError( + f"staged {kind} dataset has {episode_count} episodes; expected {expected_episode_count}" + ) + after = os.fstat(descriptor) + if (after.st_dev, after.st_ino, after.st_size, after.st_mtime_ns) != ( + before.st_dev, + before.st_ino, + before.st_size, + before.st_mtime_ns, + ): + raise RuntimeError(f"staged {kind} dataset changed during validation") + return _ValidatedDataset( + staged_name=staged_name, + final_name=final_path.name, + descriptor=descriptor, + device=before.st_dev, + inode=before.st_ino, + mtime_ns=before.st_mtime_ns, + artifact=DatasetArtifact( + path=str(final_path), + sha256=digest, + size_bytes=before.st_size, + episode_count=episode_count, + kind=kind, + ), + ) + except Exception: + os.close(descriptor) + raise + + def _require_open(self) -> None: + if self._closed: + raise RuntimeError("output transaction is closed") + + def _verify_run_log_identity(self) -> None: + if self._run_log_fd is None: + return + assert self._run_log_parent_fd is not None + assert self.run_log_path is not None + held = os.fstat(self._run_log_fd) + visible = os.stat( + self.run_log_path.name, + dir_fd=self._run_log_parent_fd, + follow_symlinks=False, + ) + if ( + not stat.S_ISREG(held.st_mode) + or not stat.S_ISREG(visible.st_mode) + or held.st_nlink != 1 + or visible.st_nlink != 1 + or _device_inode(held) != _device_inode(visible) + ): + raise RuntimeError("reserved run_log path no longer names the held run-log inode") + os.fsync(self._run_log_fd) + + def _verify_output_parent_identities(self) -> None: + _require_visible_directory_identity( + self.request_directory, + self._request_dir_fd, + "request root", + ) + _require_visible_directory_identity( + self.dataset_path.parent, + self._dataset_parent_fd, + "dataset parent", + ) + if self.run_log_path is not None: + assert self._run_log_parent_fd is not None + _require_visible_directory_identity( + self.run_log_path.parent, + self._run_log_parent_fd, + "run_log parent", + ) + + def _verify_published_dataset(self, item: _ValidatedDataset) -> None: + held = os.fstat(item.descriptor) + visible = os.stat(item.final_name, dir_fd=self._dataset_parent_fd, follow_symlinks=False) + if ( + not stat.S_ISREG(visible.st_mode) + or held.st_nlink != 1 + or visible.st_nlink != 1 + or _device_inode(held) != (item.device, item.inode) + or _device_inode(visible) != (item.device, item.inode) + or held.st_size != item.artifact.size_bytes + or visible.st_size != item.artifact.size_bytes + or held.st_mtime_ns != item.mtime_ns + or visible.st_mtime_ns != item.mtime_ns + ): + raise RuntimeError("published dataset path no longer names the validated inode") + if _sha256_fd(item.descriptor, expected_size=item.artifact.size_bytes) != item.artifact.sha256: + raise RuntimeError("published dataset content changed after validation") + + def _unlink_staged_dataset_inode(self, item: _ValidatedDataset) -> None: + matches: list[str] = [] + names = os.listdir(self._staging_fd) + if len(names) > 32: + raise RuntimeError("private staging directory contains too many entries during publication") + for name in names: + visible = os.stat(name, dir_fd=self._staging_fd, follow_symlinks=False) + if _device_inode(visible) == (item.device, item.inode): + matches.append(name) + if len(matches) > 1: + raise RuntimeError("validated dataset acquired unexpected additional staging links") + if matches: + os.unlink(matches[0], dir_fd=self._staging_fd) + + +def _absolute_lexical_path(path: str | Path) -> Path: + candidate = Path(path).expanduser() + if not candidate.is_absolute(): + candidate = Path.cwd() / candidate + return Path(os.path.normpath(candidate)) + + +def _device_inode(file_stat: os.stat_result) -> tuple[int, int]: + return file_stat.st_dev, file_stat.st_ino + + +def _relative_output_path(path: Path, request_directory: Path, label: str) -> PurePath: + try: + relative = path.relative_to(request_directory) + except ValueError as exc: + raise ValueError(f"{label} path must be below the request directory") from exc + if relative == PurePath(".") or not relative.name: + raise ValueError(f"{label} path must name a file") + if any(part in ("", ".", "..") for part in relative.parts): + raise ValueError(f"{label} path contains an unsafe component") + return relative + + +def _directory_open_flags() -> int: + return os.O_RDONLY | os.O_CLOEXEC | os.O_DIRECTORY | os.O_NOFOLLOW + + +def _open_absolute_directory(path: Path) -> int: + if not path.is_absolute(): + raise ValueError("directory anchor must be absolute") + descriptor = os.open("/", _directory_open_flags()) + try: + for component in path.parts[1:]: + next_descriptor = os.open(component, _directory_open_flags(), dir_fd=descriptor) + os.close(descriptor) + descriptor = next_descriptor + return descriptor + except Exception: + os.close(descriptor) + raise + + +def _open_relative_directory(anchor_fd: int, components: tuple[str, ...], *, create: bool) -> int: + descriptor = os.dup(anchor_fd) + try: + for component in components: + if component in ("", ".", "..") or "/" in component: + raise ValueError("output directory contains an unsafe component") + try: + next_descriptor = os.open(component, _directory_open_flags(), dir_fd=descriptor) + except FileNotFoundError: + if not create: + raise + with suppress(FileExistsError): + os.mkdir(component, mode=0o700, dir_fd=descriptor) + next_descriptor = os.open(component, _directory_open_flags(), dir_fd=descriptor) + _fsync_directory(descriptor) + os.close(descriptor) + descriptor = next_descriptor + return descriptor + except Exception: + os.close(descriptor) + raise + + +def _require_missing_entry(name: str, parent_fd: int, label: str) -> None: + try: + os.stat(name, dir_fd=parent_fd, follow_symlinks=False) + except FileNotFoundError: + return + raise FileExistsError(f"refusing to overwrite existing {label} output: {name}") + + +def _require_visible_fd_identity(name: str, parent_fd: int, descriptor: int) -> None: + held = os.fstat(descriptor) + visible = os.stat(name, dir_fd=parent_fd, follow_symlinks=False) + if ( + not stat.S_ISREG(held.st_mode) + or not stat.S_ISREG(visible.st_mode) + or held.st_nlink != 1 + or visible.st_nlink != 1 + or _device_inode(held) != _device_inode(visible) + ): + raise RuntimeError("exclusively created output path no longer names its held inode") + + +def _require_visible_directory_identity(path: Path, held_fd: int, label: str) -> None: + visible_fd = _open_absolute_directory(path) + try: + if _device_inode(os.fstat(visible_fd)) != _device_inode(os.fstat(held_fd)): + raise RuntimeError(f"visible {label} path no longer names its held directory inode") + finally: + os.close(visible_fd) + + +def _entry_exists(name: str, parent_fd: int) -> bool: + try: + os.stat(name, dir_fd=parent_fd, follow_symlinks=False) + except FileNotFoundError: + return False + return True + + +def _create_staging_directory(parent_fd: int) -> tuple[str, int]: + for _ in range(_MAX_STAGING_NAME_ATTEMPTS): + name = f".autodata-{secrets.token_hex(16)}.staging" + try: + os.mkdir(name, mode=0o700, dir_fd=parent_fd) + except FileExistsError: + continue + descriptor = os.open(name, _directory_open_flags(), dir_fd=parent_fd) + os.fchmod(descriptor, 0o700) + _fsync_directory(parent_fd) + return name, descriptor + raise FileExistsError("could not allocate a private dataset staging directory") + + +def _open_exclusive_run_log(name: str, parent_fd: int) -> int: + flags = os.O_WRONLY | os.O_CREAT | os.O_EXCL | os.O_APPEND | os.O_CLOEXEC | os.O_NOFOLLOW + descriptor = os.open(name, flags, 0o600, dir_fd=parent_fd) + os.fchmod(descriptor, 0o600) + return descriptor + + +def _sha256_fd(descriptor: int, *, expected_size: int) -> str: + digest = hashlib.sha256() + offset = 0 + while offset < expected_size: + chunk = os.pread(descriptor, min(_HASH_CHUNK_BYTES, expected_size - offset), offset) + if not chunk: + raise OSError("staged dataset ended while it was being hashed") + digest.update(chunk) + offset += len(chunk) + return digest.hexdigest() + + +def _link_fd_to_name(source_fd: int, destination_dir_fd: int, destination_name: str) -> None: + """Hard-link the exact held inode through its process-local descriptor identity.""" + + os.link( + f"/proc/self/fd/{source_fd}", + destination_name, + dst_dir_fd=destination_dir_fd, + follow_symlinks=True, + ) + + +def _fsync_directory(descriptor: int) -> None: + descriptor_stat = os.fstat(descriptor) + if not stat.S_ISDIR(descriptor_stat.st_mode): + raise ValueError("directory durability operation received a non-directory descriptor") + try: + os.fsync(descriptor) + except OSError as exc: + raise RuntimeError( + "output filesystem does not support the required durable directory fsync " + f"(errno={exc.errno}: {exc.strerror})" + ) from exc + + +def _probe_output_filesystem(staging_fd: int, parent_fd: int) -> None: + """Exercise the exact HDF5, descriptor-link, and durability primitives before generation.""" + + token = secrets.token_hex(12) + staged_name = f".autodata-probe-{token}.hdf5" + published_name = f".autodata-probe-{token}.published" + descriptor: int | None = None + staged_exists = False + published_exists = False + try: + try: + import h5py + except ImportError as exc: + raise RuntimeError("h5py is required for the autonomous output filesystem probe") from exc + staged_path = f"/proc/self/fd/{staging_fd}/{staged_name}" + with h5py.File(staged_path, "x") as dataset: + dataset.create_group("data") + staged_exists = True + descriptor = os.open(staged_name, os.O_RDONLY | os.O_CLOEXEC | os.O_NOFOLLOW, dir_fd=staging_fd) + source_stat = os.fstat(descriptor) + if not stat.S_ISREG(source_stat.st_mode) or source_stat.st_nlink != 1: + raise RuntimeError("filesystem probe did not create a private single-link regular HDF5 file") + os.fsync(descriptor) + if hdf5_episode_count(descriptor, kind="filesystem probe") != 0: + raise RuntimeError("filesystem probe HDF5 episode group was not empty") + _link_fd_to_name(descriptor, parent_fd, published_name) + published_exists = True + published_stat = os.stat(published_name, dir_fd=parent_fd, follow_symlinks=False) + if _device_inode(published_stat) != _device_inode(source_stat): + raise RuntimeError("filesystem probe hard link did not preserve the held inode") + _fsync_directory(parent_fd) + os.unlink(published_name, dir_fd=parent_fd) + published_exists = False + os.unlink(staged_name, dir_fd=staging_fd) + staged_exists = False + _fsync_directory(staging_fd) + _fsync_directory(parent_fd) + except Exception as exc: + raise RuntimeError( + "output filesystem does not support the required descriptor-safe HDF5 transaction: " + f"{type(exc).__name__}: {exc}" + ) from exc + finally: + if descriptor is not None: + os.close(descriptor) + if published_exists: + with suppress(OSError): + os.unlink(published_name, dir_fd=parent_fd) + if staged_exists: + with suppress(OSError): + os.unlink(staged_name, dir_fd=staging_fd) + + +def _cleanup_private_staging(staging_fd: int, staging_name: str, parent_fd: int) -> None: + issue: BaseException | None = None + try: + names = os.listdir(staging_fd) + if len(names) > 32: + raise OSError("private staging directory contains too many unexpected entries") + for name in names: + if name in ("", ".", "..") or "/" in name: + raise OSError("private staging directory contains an unsafe entry") + entry = os.stat(name, dir_fd=staging_fd, follow_symlinks=False) + if stat.S_ISDIR(entry.st_mode): + os.rmdir(name, dir_fd=staging_fd) + else: + os.unlink(name, dir_fd=staging_fd) + except BaseException as exc: + issue = exc + finally: + os.close(staging_fd) + try: + os.rmdir(staging_name, dir_fd=parent_fd) + _fsync_directory(parent_fd) + except BaseException as exc: + if issue is None: + issue = exc + if issue is not None: + raise issue + + +def _distinct_fds(*descriptors: int | None) -> tuple[int, ...]: + return tuple(dict.fromkeys(descriptor for descriptor in descriptors if descriptor is not None)) + + +def _close_distinct_fds_best_effort(*descriptors: int | None) -> None: + for descriptor in _distinct_fds(*descriptors): + with suppress(OSError): + os.close(descriptor) diff --git a/isaac_autodata_core/autonomous/run_log.py b/isaac_autodata_core/autonomous/run_log.py new file mode 100644 index 0000000..3bba938 --- /dev/null +++ b/isaac_autodata_core/autonomous/run_log.py @@ -0,0 +1,229 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Append-only, bounded JSON Lines run log for autonomous generation.""" + +from __future__ import annotations + +import fcntl +import itertools +import json +import os +import stat +import threading +import traceback +from collections.abc import Mapping +from pathlib import Path +from typing import Any + +from isaac_autodata_core.autonomous.task_motion import ExecutionEvent, JsonValue, TaskMotionPlan + +DEFAULT_MAX_RECORD_BYTES = 4_000_000 +MAX_RUN_RECORD_COLLECTION_ITEMS = 100_000 +MAX_RUN_RECORD_JSON_NODES = 200_000 + + +class RunLogWriteUncertainError(RuntimeError): + """A run-log write may be partial or durable, so no later append is safe.""" + + +def safe_exception_record(exc: BaseException, *, include_traceback: bool = False) -> dict[str, str]: + """Return a bounded exception summary without process environment or object repr data. + + Args: + exc: Exception to summarize. + include_traceback: Include a bounded formatted traceback for explicit debug runs. + """ + + message = str(exc).replace("\x00", "").strip() + record = { + "exception_type": f"{type(exc).__module__}.{type(exc).__qualname__}"[:512], + "message": message[:2048], + } + if include_traceback: + record["traceback"] = "".join(traceback.format_exception(exc))[-16_384:] + return record + + +class RunLogWriter: + """Synchronously append validated records to a JSON Lines run log. + + The writer is safe for multiple generation tasks in one process. It holds one append-only file + descriptor for its lifetime, writes exactly one line under a process-local lock, and calls + :func:`os.fsync` so completed records survive a later process crash. A descriptor reserved by a + descriptor-safe output transaction can be supplied directly, avoiding a path re-open. + """ + + def __init__( + self, + path: str | Path, + *, + fd: int | None = None, + close_fd: bool = True, + max_record_bytes: int = DEFAULT_MAX_RECORD_BYTES, + ) -> None: + self.path = Path(path).expanduser().absolute() + if self.path.suffix.lower() != ".jsonl": + raise ValueError("run log path must end in .jsonl") + if isinstance(max_record_bytes, bool) or not isinstance(max_record_bytes, int) or max_record_bytes < 1024: + raise ValueError("max_record_bytes must be an integer of at least 1024") + if type(close_fd) is not bool: + raise TypeError("close_fd must be a boolean") + self.max_record_bytes = max_record_bytes + self._lock = threading.Lock() + self._close_fd = close_fd + self._closed = False + self._poisoned = False + if fd is None: + self.path.parent.mkdir(parents=True, exist_ok=True) + self._fd = os.open( + self.path, + os.O_WRONLY | os.O_CREAT | os.O_APPEND | os.O_CLOEXEC | os.O_NOFOLLOW, + 0o600, + ) + else: + if isinstance(fd, bool) or not isinstance(fd, int) or fd < 0: + raise ValueError("fd must be a non-negative file descriptor") + descriptor_stat = os.fstat(fd) + if not stat.S_ISREG(descriptor_stat.st_mode): + raise ValueError("run log descriptor must refer to a regular file") + descriptor_flags = fcntl.fcntl(fd, fcntl.F_GETFL) + if not descriptor_flags & os.O_APPEND: + raise ValueError("run log descriptor must be opened with O_APPEND") + if descriptor_flags & os.O_ACCMODE == os.O_RDONLY: + raise ValueError("run log descriptor must be writable") + self._fd = fd + + def append(self, record: Mapping[str, Any] | ExecutionEvent | TaskMotionPlan) -> None: + """Validate and durably append one run-log record. + + Args: + record: JSON-compatible mapping or a supported typed contract. + """ + + if isinstance(record, (ExecutionEvent, TaskMotionPlan)): + payload = record.to_dict() + elif isinstance(record, Mapping): + payload = dict(record) + else: + raise TypeError(f"unsupported run-log record {type(record).__name__}") + normalized = normalize_run_record(payload) + assert isinstance(normalized, dict) + serialized = _serialize_normalized_json(normalized) + encoded = (serialized + "\n").encode("utf-8") + if len(encoded) > self.max_record_bytes: + raise ValueError(f"run-log record is {len(encoded)} bytes; maximum is {self.max_record_bytes}") + + with self._lock: + if self._closed: + raise RuntimeError("run-log writer is closed") + if self._poisoned: + raise RunLogWriteUncertainError( + "run log is poisoned by an earlier append/fsync failure; inspect and repair its tail" + ) + try: + remaining = memoryview(encoded) + while remaining: + written = os.write(self._fd, remaining) + if written <= 0: + raise OSError("run-log append made no progress") + remaining = remaining[written:] + os.fsync(self._fd) + except BaseException as exc: + self._poisoned = True + raise RunLogWriteUncertainError( + "run-log append/fsync durability is unknown; no later log write or output commit is safe" + ) from exc + + def close(self) -> None: + """Close the held descriptor when this writer owns it.""" + + with self._lock: + if self._closed: + return + self._closed = True + if self._close_fd: + os.close(self._fd) + + def __enter__(self) -> RunLogWriter: + """Return this writer as a context manager.""" + + return self + + def __exit__(self, *_args: object) -> None: + """Close the writer when leaving a context manager.""" + + self.close() + + +def normalize_run_record(value: Any, *, max_serialized_bytes: int | None = None) -> JsonValue: + """Normalize one value under the run log's strict JSON and optional byte bounds. + + Args: + value: Candidate JSON-compatible value. + max_serialized_bytes: Optional maximum canonical UTF-8 payload size. + + Returns: + A detached JSON-compatible value using sorted string mapping keys and lists for sequences. + + Raises: + ValueError: If the value is malformed, too deep, too large, or contains too many items. + """ + + if max_serialized_bytes is not None and ( + isinstance(max_serialized_bytes, bool) or not isinstance(max_serialized_bytes, int) or max_serialized_bytes <= 0 + ): + raise ValueError("max_serialized_bytes must be null or a positive integer") + normalized = _normalize_json(value) + if max_serialized_bytes is not None: + serialized_bytes = len(_serialize_normalized_json(normalized).encode("utf-8")) + if serialized_bytes > max_serialized_bytes: + raise ValueError(f"run-record value is {serialized_bytes} bytes; maximum is {max_serialized_bytes}") + return normalized + + +def _serialize_normalized_json(value: JsonValue) -> str: + """Serialize an already-normalized value exactly as the append-only run log does.""" + + return json.dumps( + value, + sort_keys=True, + separators=(",", ":"), + ensure_ascii=False, + allow_nan=False, + ) + + +def _normalize_json(value: Any, depth: int = 0, node_budget: list[int] | None = None) -> JsonValue: + if node_budget is None: + node_budget = [MAX_RUN_RECORD_JSON_NODES] + node_budget[0] -= 1 + if node_budget[0] < 0: + raise ValueError("run-log record exceeds maximum JSON node count") + if depth > 12: + raise ValueError("run-log record exceeds maximum nesting depth") + if value is None or isinstance(value, (str, bool, int)): + return value + if isinstance(value, float): + if value != value or value in (float("inf"), float("-inf")): + raise ValueError("run-log numbers must be finite") + return value + if isinstance(value, Mapping): + if len(value) > MAX_RUN_RECORD_COLLECTION_ITEMS: + raise ValueError("run-log mapping exceeds maximum item count") + keys = list(itertools.islice(iter(value), MAX_RUN_RECORD_COLLECTION_ITEMS + 1)) + if len(keys) > MAX_RUN_RECORD_COLLECTION_ITEMS or len(keys) != len(value): + raise ValueError("run-log mapping exceeds or misreports maximum item count") + if any(not isinstance(key, str) for key in keys): + raise ValueError("run-log mapping keys must be strings") + result: dict[str, JsonValue] = {} + for key in sorted(keys): + result[key] = _normalize_json(value[key], depth + 1, node_budget) + return result + if isinstance(value, (list, tuple)): + if len(value) > MAX_RUN_RECORD_COLLECTION_ITEMS: + raise ValueError("run-log sequence exceeds maximum item count") + return [_normalize_json(item, depth + 1, node_budget) for item in value] + raise ValueError(f"run log contains unsupported value type {type(value).__name__}") diff --git a/isaac_autodata_core/autonomous/task_motion.py b/isaac_autodata_core/autonomous/task_motion.py new file mode 100644 index 0000000..3885e5e --- /dev/null +++ b/isaac_autodata_core/autonomous/task_motion.py @@ -0,0 +1,880 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Versioned, planner-neutral contracts for autonomous generation. + +The types in this module deliberately depend only on the Python standard library. Planner and +simulator adapters convert their native tensors and commands at this boundary; native cuRobo, +ScheduleStream, Isaac Lab, or Pydantic objects must not leak into a serialized plan. +""" + +from __future__ import annotations + +import hashlib +import json +import math +from collections.abc import Mapping, Sequence +from dataclasses import dataclass, field +from enum import StrEnum +from typing import Any, ClassVar, TypeAlias + +JsonScalar: TypeAlias = str | int | float | bool | None +JsonValue: TypeAlias = JsonScalar | list["JsonValue"] | dict[str, "JsonValue"] +Matrix4: TypeAlias = tuple[ + tuple[float, float, float, float], + tuple[float, float, float, float], + tuple[float, float, float, float], + tuple[float, float, float, float], +] +IDENTITY_MATRIX4: Matrix4 = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + +TASK_MOTION_PLAN_SCHEMA_VERSION = 1 +SCENE_SNAPSHOT_SCHEMA_VERSION = 1 +EXECUTION_EVENT_SCHEMA_VERSION = 1 + + +def _canonical_json(value: Any) -> str: + return json.dumps(value, sort_keys=True, separators=(",", ":"), ensure_ascii=False, allow_nan=False) + + +def make_stable_id(namespace: str, *parts: Any) -> str: + """Return a compact deterministic identifier for canonical JSON-compatible inputs. + + Args: + namespace: Human-readable ID prefix. + parts: Values contributing to the content hash. + """ + + assert namespace and namespace.strip(), "namespace must not be empty" + digest = hashlib.sha256(_canonical_json(list(parts)).encode("utf-8")).hexdigest()[:20] + return f"{namespace}-{digest}" + + +def _require_text(value: str, field_name: str, *, maximum: int = 512) -> None: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} must be a non-empty string") + if len(value) > maximum: + raise ValueError(f"{field_name} exceeds {maximum} characters") + if "\x00" in value: + raise ValueError(f"{field_name} must not contain NUL") + + +def _finite_float(value: int | float, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{field_name} must be a number") + result = float(value) + if not math.isfinite(result): + raise ValueError(f"{field_name} must be finite") + return result + + +def _number_tuple(values: Sequence[int | float], field_name: str) -> tuple[float, ...]: + if isinstance(values, (str, bytes)): + raise ValueError(f"{field_name} must be a numeric sequence") + return tuple(_finite_float(value, f"{field_name}[{index}]") for index, value in enumerate(values)) + + +def matrix4(value: Sequence[Sequence[int | float]], field_name: str = "pose") -> Matrix4: + """Validate and normalize one approximately rigid homogeneous transform. + + Args: + value: Four rows with four finite numeric values each. + field_name: Field path used in validation messages. + """ + + if isinstance(value, (str, bytes)) or len(value) != 4: + raise ValueError(f"{field_name} must have shape [4, 4]") + rows = tuple(_number_tuple(row, f"{field_name}[{index}]") for index, row in enumerate(value)) + if any(len(row) != 4 for row in rows): + raise ValueError(f"{field_name} must have shape [4, 4]") + expected_last_row = (0.0, 0.0, 0.0, 1.0) + if any(abs(actual - expected) > 1e-5 for actual, expected in zip(rows[3], expected_last_row)): + raise ValueError(f"{field_name} must have homogeneous last row [0, 0, 0, 1]") + + rotation = tuple(row[:3] for row in rows[:3]) + for column in range(3): + norm = sum(rotation[row][column] ** 2 for row in range(3)) + if abs(norm - 1.0) > 2e-3: + raise ValueError(f"{field_name} rotation columns must have unit norm") + for other in range(column + 1, 3): + dot = sum(rotation[row][column] * rotation[row][other] for row in range(3)) + if abs(dot) > 2e-3: + raise ValueError(f"{field_name} rotation columns must be orthogonal") + determinant = ( + rotation[0][0] * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) + - rotation[0][1] * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) + + rotation[0][2] * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) + ) + if abs(determinant - 1.0) > 2e-3: + raise ValueError(f"{field_name} rotation determinant must be +1") + return rows # type: ignore[return-value] + + +def matrix4_multiply(first: Matrix4, second: Matrix4) -> Matrix4: + """Compose two rigid homogeneous transforms.""" + + return tuple( + tuple(sum(first[row][inner] * second[inner][column] for inner in range(4)) for column in range(4)) + for row in range(4) + ) # type: ignore[return-value] + + +def matrix4_inverse(value: Matrix4) -> Matrix4: + """Return the rigid inverse of a homogeneous transform.""" + + rotation_transpose = tuple(tuple(value[column][row] for column in range(3)) for row in range(3)) + translation = tuple(value[row][3] for row in range(3)) + inverse_translation = tuple( + -sum(rotation_transpose[row][column] * translation[column] for column in range(3)) for row in range(3) + ) + return tuple( + tuple(rotation_transpose[row][column] for column in range(3)) + (inverse_translation[row],) for row in range(3) + ) + ( + (0.0, 0.0, 0.0, 1.0), + ) + + +def matrix4_error(first: Matrix4, second: Matrix4) -> tuple[float, float]: + """Return translation [m] and rotation [rad] error between rigid transforms.""" + + position_error = math.sqrt(sum((first[row][3] - second[row][3]) ** 2 for row in range(3))) + relative_trace = sum(first[row][column] * second[row][column] for row in range(3) for column in range(3)) + cosine = max(-1.0, min(1.0, (relative_trace - 1.0) / 2.0)) + return position_error, math.acos(cosine) + + +def _json_value(value: Any, field_name: str = "metadata", depth: int = 0) -> JsonValue: + if depth > 10: + raise ValueError(f"{field_name} exceeds maximum nesting depth") + if value is None or isinstance(value, (str, bool, int)): + return value + if isinstance(value, float): + return _finite_float(value, field_name) + if isinstance(value, Mapping): + result: dict[str, JsonValue] = {} + for key in sorted(value): + if not isinstance(key, str): + raise ValueError(f"{field_name} keys must be strings") + result[key] = _json_value(value[key], f"{field_name}.{key}", depth + 1) + return result + if isinstance(value, Sequence) and not isinstance(value, (str, bytes)): + return [_json_value(item, f"{field_name}[{index}]", depth + 1) for index, item in enumerate(value)] + raise ValueError(f"{field_name} contains unsupported value type {type(value).__name__}") + + +@dataclass(frozen=True, order=True) +class GoalPredicate: + """One planner-neutral relational predicate.""" + + relation: str + subject: str + target: str | None = None + + def __post_init__(self) -> None: + _require_text(self.relation, "relation", maximum=128) + _require_text(self.subject, "subject") + if self.target is not None: + _require_text(self.target, "target") + + def to_dict(self) -> dict[str, str]: + result = {"relation": self.relation, "subject": self.subject} + if self.target is not None: + result["target"] = self.target + return result + + @classmethod + def from_dict(cls, value: Mapping[str, Any]) -> GoalPredicate: + return cls(relation=value["relation"], subject=value["subject"], target=value.get("target")) + + +@dataclass(frozen=True) +class SceneObjectSnapshot: + """Planner-neutral state for one semantic scene object.""" + + semantic_id: str + scene_id: str + pose: Matrix4 + geometry_ref: str | None = None + geometry_digest: str | None = None + roles: tuple[str, ...] = () + + def __post_init__(self) -> None: + _require_text(self.semantic_id, "semantic_id") + _require_text(self.scene_id, "scene_id") + object.__setattr__(self, "pose", matrix4(self.pose, f"objects[{self.semantic_id}].pose")) + if self.geometry_ref is not None: + _require_text(self.geometry_ref, "geometry_ref", maximum=2048) + if self.geometry_digest is not None: + _require_text(self.geometry_digest, "geometry_digest", maximum=256) + object.__setattr__(self, "roles", tuple(sorted(set(self.roles)))) + + def to_dict(self) -> dict[str, Any]: + return { + "geometry_digest": self.geometry_digest, + "geometry_ref": self.geometry_ref, + "pose": [list(row) for row in self.pose], + "roles": list(self.roles), + "scene_id": self.scene_id, + "semantic_id": self.semantic_id, + } + + @classmethod + def from_dict(cls, value: Mapping[str, Any]) -> SceneObjectSnapshot: + return cls( + semantic_id=value["semantic_id"], + scene_id=value["scene_id"], + pose=value["pose"], + geometry_ref=value.get("geometry_ref"), + geometry_digest=value.get("geometry_digest"), + roles=tuple(value.get("roles", ())), + ) + + +@dataclass(frozen=True) +class RobotStateSnapshot: + """Planner-neutral embodiment state at one instant.""" + + robot_id: str + joint_names: tuple[str, ...] + joint_positions: tuple[float, ...] + eef_poses: Mapping[str, Matrix4] + held_objects: Mapping[str, str | None] = field(default_factory=dict) + + def __post_init__(self) -> None: + _require_text(self.robot_id, "robot_id") + if not self.joint_names or len(self.joint_names) != len(self.joint_positions): + raise ValueError("joint_names and joint_positions must be non-empty and have equal length") + if len(set(self.joint_names)) != len(self.joint_names): + raise ValueError("joint_names must be unique") + for name in self.joint_names: + _require_text(name, "joint_name") + object.__setattr__(self, "joint_positions", _number_tuple(self.joint_positions, "joint_positions")) + if not self.eef_poses: + raise ValueError("eef_poses must not be empty") + normalized_poses: dict[str, Matrix4] = {} + for name, pose in sorted(self.eef_poses.items()): + _require_text(name, "eef_name") + normalized_poses[name] = matrix4(pose, f"eef_poses[{name}]") + object.__setattr__(self, "eef_poses", normalized_poses) + normalized_holds: dict[str, str | None] = {} + for name, held_object in sorted(self.held_objects.items()): + if name not in normalized_poses: + raise ValueError(f"held_objects references unknown EEF {name!r}") + if held_object is not None: + _require_text(held_object, f"held_objects[{name}]") + normalized_holds[name] = held_object + object.__setattr__(self, "held_objects", normalized_holds) + + def to_dict(self) -> dict[str, Any]: + return { + "eef_poses": {name: [list(row) for row in pose] for name, pose in self.eef_poses.items()}, + "held_objects": dict(self.held_objects), + "joint_names": list(self.joint_names), + "joint_positions": list(self.joint_positions), + "robot_id": self.robot_id, + } + + @classmethod + def from_dict(cls, value: Mapping[str, Any]) -> RobotStateSnapshot: + return cls( + robot_id=value["robot_id"], + joint_names=tuple(value["joint_names"]), + joint_positions=tuple(value["joint_positions"]), + eef_poses=value["eef_poses"], + held_objects=value.get("held_objects", {}), + ) + + +@dataclass(frozen=True) +class SceneSnapshot: + """Complete planner-neutral state used for one planning attempt.""" + + snapshot_id: str + captured_at_s: float + env_id: int + robot: RobotStateSnapshot + objects: tuple[SceneObjectSnapshot, ...] + metadata: Mapping[str, JsonValue] = field(default_factory=dict) + schema_version: int = SCENE_SNAPSHOT_SCHEMA_VERSION + + def __post_init__(self) -> None: + _require_text(self.snapshot_id, "snapshot_id") + object.__setattr__(self, "captured_at_s", _finite_float(self.captured_at_s, "captured_at_s")) + if self.captured_at_s < 0: + raise ValueError("captured_at_s must be non-negative") + if isinstance(self.env_id, bool) or not isinstance(self.env_id, int) or self.env_id < 0: + raise ValueError("env_id must be a non-negative integer") + if self.schema_version != SCENE_SNAPSHOT_SCHEMA_VERSION: + raise ValueError(f"unsupported scene snapshot schema version {self.schema_version}") + ids = [item.semantic_id for item in self.objects] + if len(ids) != len(set(ids)): + raise ValueError("scene object semantic IDs must be unique") + object.__setattr__(self, "objects", tuple(sorted(self.objects, key=lambda item: item.semantic_id))) + metadata = _json_value(self.metadata) + assert isinstance(metadata, dict) + object.__setattr__(self, "metadata", metadata) + + def to_dict(self) -> dict[str, Any]: + return { + "captured_at_s": self.captured_at_s, + "env_id": self.env_id, + "metadata": dict(self.metadata), + "objects": [item.to_dict() for item in self.objects], + "robot": self.robot.to_dict(), + "schema_version": self.schema_version, + "snapshot_id": self.snapshot_id, + } + + @property + def digest(self) -> str: + return hashlib.sha256(_canonical_json(self.to_dict()).encode("utf-8")).hexdigest() + + @classmethod + def from_dict(cls, value: Mapping[str, Any]) -> SceneSnapshot: + return cls( + snapshot_id=value["snapshot_id"], + captured_at_s=value["captured_at_s"], + env_id=value["env_id"], + robot=RobotStateSnapshot.from_dict(value["robot"]), + objects=tuple(SceneObjectSnapshot.from_dict(item) for item in value["objects"]), + metadata=value.get("metadata", {}), + schema_version=value.get("schema_version", SCENE_SNAPSHOT_SCHEMA_VERSION), + ) + + +class SegmentKind(StrEnum): + CARTESIAN_TRAJECTORY = "cartesian_trajectory" + JOINT_TRAJECTORY = "joint_trajectory" + GRIPPER_COMMAND = "gripper_command" + ATTACH_INTENT = "attach_intent" + DETACH_INTENT = "detach_intent" + WAIT = "wait" + BARRIER = "barrier" + CONCURRENT_GROUP = "concurrent_group" + + +class GripperCommandMode(StrEnum): + OPEN = "open" + CLOSE = "close" + POSITION = "position" + EFFORT = "effort" + + +@dataclass(frozen=True, kw_only=True) +class PlanSegment: + """Fields shared by every task-motion segment.""" + + kind: ClassVar[SegmentKind] + segment_id: str + depends_on: tuple[str, ...] = () + start_time_s: float | None = None + duration_s: float | None = None + assumptions: tuple[GoalPredicate, ...] = () + expected_postconditions: tuple[GoalPredicate, ...] = () + metadata: Mapping[str, JsonValue] = field(default_factory=dict) + + def __post_init__(self) -> None: + _require_text(self.segment_id, "segment_id") + for dependency in self.depends_on: + _require_text(dependency, "depends_on") + if len(set(self.depends_on)) != len(self.depends_on): + raise ValueError(f"segment {self.segment_id!r} has duplicate dependencies") + if self.start_time_s is not None: + value = _finite_float(self.start_time_s, "start_time_s") + if value < 0: + raise ValueError("start_time_s must be non-negative") + object.__setattr__(self, "start_time_s", value) + if self.duration_s is not None: + value = _finite_float(self.duration_s, "duration_s") + if value < 0: + raise ValueError("duration_s must be non-negative") + object.__setattr__(self, "duration_s", value) + metadata = _json_value(self.metadata) + assert isinstance(metadata, dict) + object.__setattr__(self, "metadata", metadata) + + +@dataclass(frozen=True, kw_only=True) +class CartesianTrajectorySegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.CARTESIAN_TRAJECTORY + eef_name: str + frame: str + poses: tuple[Matrix4, ...] + joint_seed_names: tuple[str, ...] = () + joint_seeds: tuple[tuple[float, ...], ...] = () + + def __post_init__(self) -> None: + super().__post_init__() + _require_text(self.eef_name, "eef_name") + _require_text(self.frame, "frame") + if not self.poses: + raise ValueError("Cartesian trajectory must contain at least one pose") + object.__setattr__( + self, + "poses", + tuple( + matrix4(pose, f"segments[{self.segment_id}].poses[{index}]") for index, pose in enumerate(self.poses) + ), + ) + normalized_seeds = tuple( + _number_tuple(seed, f"segments[{self.segment_id}].joint_seeds[{index}]") + for index, seed in enumerate(self.joint_seeds) + ) + if normalized_seeds and len(normalized_seeds) != len(self.poses): + raise ValueError("joint_seeds must be empty or align one-to-one with Cartesian poses") + if normalized_seeds: + if not self.joint_seed_names or len(set(self.joint_seed_names)) != len(self.joint_seed_names): + raise ValueError("joint_seed_names must be non-empty and unique when joint_seeds are present") + for name in self.joint_seed_names: + _require_text(name, "joint_seed_name") + if any(len(seed) != len(self.joint_seed_names) for seed in normalized_seeds): + raise ValueError("every joint seed must match joint_seed_names length") + elif self.joint_seed_names: + raise ValueError("joint_seed_names must be empty when joint_seeds are absent") + object.__setattr__(self, "joint_seeds", normalized_seeds) + + +@dataclass(frozen=True, kw_only=True) +class JointTrajectorySegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.JOINT_TRAJECTORY + joint_names: tuple[str, ...] + positions: tuple[tuple[float, ...], ...] + timestamps_s: tuple[float, ...] = () + attached_object: str | None = None + + def __post_init__(self) -> None: + super().__post_init__() + if not self.joint_names or len(set(self.joint_names)) != len(self.joint_names): + raise ValueError("joint_names must be non-empty and unique") + for name in self.joint_names: + _require_text(name, "joint_name") + if not self.positions: + raise ValueError("joint trajectory must contain at least one position") + normalized_positions = tuple( + _number_tuple(position, f"segments[{self.segment_id}].positions[{index}]") + for index, position in enumerate(self.positions) + ) + if any(len(position) != len(self.joint_names) for position in normalized_positions): + raise ValueError("every joint position must match joint_names length") + object.__setattr__(self, "positions", normalized_positions) + timestamps = _number_tuple(self.timestamps_s, "timestamps_s") + if timestamps and len(timestamps) != len(normalized_positions): + raise ValueError("timestamps_s must be empty or align with joint positions") + if any(current < previous for previous, current in zip(timestamps, timestamps[1:])): + raise ValueError("timestamps_s must be monotonic") + if timestamps and timestamps[0] < 0: + raise ValueError("timestamps_s must be non-negative") + object.__setattr__(self, "timestamps_s", timestamps) + if self.attached_object is not None: + _require_text(self.attached_object, "attached_object") + + +@dataclass(frozen=True, kw_only=True) +class GripperCommandSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.GRIPPER_COMMAND + eef_name: str + command: GripperCommandMode + value: float | None = None + settle_steps: int = 1 + + def __post_init__(self) -> None: + super().__post_init__() + _require_text(self.eef_name, "eef_name") + if not isinstance(self.command, GripperCommandMode): + object.__setattr__(self, "command", GripperCommandMode(self.command)) + if self.command in (GripperCommandMode.POSITION, GripperCommandMode.EFFORT) and self.value is None: + raise ValueError(f"gripper command {self.command.value!r} requires value") + if self.value is not None: + object.__setattr__(self, "value", _finite_float(self.value, "gripper value")) + if isinstance(self.settle_steps, bool) or not isinstance(self.settle_steps, int) or self.settle_steps < 1: + raise ValueError("settle_steps must be a positive integer") + + +@dataclass(frozen=True, kw_only=True) +class AttachIntentSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.ATTACH_INTENT + eef_name: str + object_name: str + verifier: str + + def __post_init__(self) -> None: + super().__post_init__() + _require_text(self.eef_name, "eef_name") + _require_text(self.object_name, "object_name") + _require_text(self.verifier, "verifier") + + +@dataclass(frozen=True, kw_only=True) +class DetachIntentSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.DETACH_INTENT + eef_name: str + object_name: str + verifier: str + + def __post_init__(self) -> None: + super().__post_init__() + _require_text(self.eef_name, "eef_name") + _require_text(self.object_name, "object_name") + _require_text(self.verifier, "verifier") + + +@dataclass(frozen=True, kw_only=True) +class WaitSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.WAIT + steps: int | None = None + + def __post_init__(self) -> None: + super().__post_init__() + if self.steps is None and self.duration_s is None: + raise ValueError("wait segment requires steps or duration_s") + if self.steps is not None and ( + isinstance(self.steps, bool) or not isinstance(self.steps, int) or self.steps < 1 + ): + raise ValueError("wait steps must be a positive integer") + + +@dataclass(frozen=True, kw_only=True) +class BarrierSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.BARRIER + participants: tuple[str, ...] + + def __post_init__(self) -> None: + super().__post_init__() + if not self.participants or len(set(self.participants)) != len(self.participants): + raise ValueError("barrier participants must be non-empty and unique") + for participant in self.participants: + _require_text(participant, "barrier participant") + + +@dataclass(frozen=True, kw_only=True) +class ConcurrentGroupSegment(PlanSegment): + kind: ClassVar[SegmentKind] = SegmentKind.CONCURRENT_GROUP + member_segment_ids: tuple[str, ...] + + def __post_init__(self) -> None: + super().__post_init__() + if not self.member_segment_ids or len(set(self.member_segment_ids)) != len(self.member_segment_ids): + raise ValueError("concurrent group members must be non-empty and unique") + for member in self.member_segment_ids: + _require_text(member, "concurrent group member") + + +TaskMotionSegment: TypeAlias = ( + CartesianTrajectorySegment + | JointTrajectorySegment + | GripperCommandSegment + | AttachIntentSegment + | DetachIntentSegment + | WaitSegment + | BarrierSegment + | ConcurrentGroupSegment +) + + +def _segment_base_dict(segment: PlanSegment) -> dict[str, Any]: + return { + "assumptions": [predicate.to_dict() for predicate in segment.assumptions], + "depends_on": list(segment.depends_on), + "duration_s": segment.duration_s, + "expected_postconditions": [predicate.to_dict() for predicate in segment.expected_postconditions], + "kind": segment.kind.value, + "metadata": dict(segment.metadata), + "segment_id": segment.segment_id, + "start_time_s": segment.start_time_s, + } + + +def segment_to_dict(segment: TaskMotionSegment) -> dict[str, Any]: + result = _segment_base_dict(segment) + if isinstance(segment, CartesianTrajectorySegment): + result.update({ + "eef_name": segment.eef_name, + "frame": segment.frame, + "joint_seed_names": list(segment.joint_seed_names), + "joint_seeds": [list(seed) for seed in segment.joint_seeds], + "poses": [[list(row) for row in pose] for pose in segment.poses], + }) + elif isinstance(segment, JointTrajectorySegment): + result.update({ + "attached_object": segment.attached_object, + "joint_names": list(segment.joint_names), + "positions": [list(position) for position in segment.positions], + "timestamps_s": list(segment.timestamps_s), + }) + elif isinstance(segment, GripperCommandSegment): + result.update({ + "command": segment.command.value, + "eef_name": segment.eef_name, + "settle_steps": segment.settle_steps, + "value": segment.value, + }) + elif isinstance(segment, (AttachIntentSegment, DetachIntentSegment)): + result.update({ + "eef_name": segment.eef_name, + "object_name": segment.object_name, + "verifier": segment.verifier, + }) + elif isinstance(segment, WaitSegment): + result["steps"] = segment.steps + elif isinstance(segment, BarrierSegment): + result["participants"] = list(segment.participants) + elif isinstance(segment, ConcurrentGroupSegment): + result["member_segment_ids"] = list(segment.member_segment_ids) + else: + raise TypeError(f"unsupported plan segment {type(segment).__name__}") + return result + + +def segment_from_dict(value: Mapping[str, Any]) -> TaskMotionSegment: + kind = SegmentKind(value["kind"]) + common = { + "segment_id": value["segment_id"], + "depends_on": tuple(value.get("depends_on", ())), + "start_time_s": value.get("start_time_s"), + "duration_s": value.get("duration_s"), + "assumptions": tuple(GoalPredicate.from_dict(item) for item in value.get("assumptions", ())), + "expected_postconditions": tuple( + GoalPredicate.from_dict(item) for item in value.get("expected_postconditions", ()) + ), + "metadata": value.get("metadata", {}), + } + if kind is SegmentKind.CARTESIAN_TRAJECTORY: + return CartesianTrajectorySegment( + **common, + eef_name=value["eef_name"], + frame=value["frame"], + poses=tuple(value["poses"]), + joint_seed_names=tuple(value.get("joint_seed_names", ())), + joint_seeds=tuple(tuple(seed) for seed in value.get("joint_seeds", ())), + ) + if kind is SegmentKind.JOINT_TRAJECTORY: + return JointTrajectorySegment( + **common, + joint_names=tuple(value["joint_names"]), + positions=tuple(tuple(position) for position in value["positions"]), + timestamps_s=tuple(value.get("timestamps_s", ())), + attached_object=value.get("attached_object"), + ) + if kind is SegmentKind.GRIPPER_COMMAND: + return GripperCommandSegment( + **common, + eef_name=value["eef_name"], + command=GripperCommandMode(value["command"]), + value=value.get("value"), + settle_steps=value.get("settle_steps", 1), + ) + if kind is SegmentKind.ATTACH_INTENT: + return AttachIntentSegment( + **common, + eef_name=value["eef_name"], + object_name=value["object_name"], + verifier=value["verifier"], + ) + if kind is SegmentKind.DETACH_INTENT: + return DetachIntentSegment( + **common, + eef_name=value["eef_name"], + object_name=value["object_name"], + verifier=value["verifier"], + ) + if kind is SegmentKind.WAIT: + return WaitSegment(**common, steps=value.get("steps")) + if kind is SegmentKind.BARRIER: + return BarrierSegment(**common, participants=tuple(value["participants"])) + if kind is SegmentKind.CONCURRENT_GROUP: + return ConcurrentGroupSegment(**common, member_segment_ids=tuple(value["member_segment_ids"])) + raise AssertionError(f"unhandled segment kind {kind}") + + +@dataclass(frozen=True) +class TaskMotionPlan: + """Executable task-and-motion plan with preserved symbolic and temporal structure.""" + + plan_id: str + request_digest: str + snapshot_digest: str + backend: str + backend_version: str + seed: int + segments: tuple[TaskMotionSegment, ...] + goal: tuple[GoalPredicate, ...] + metadata: Mapping[str, JsonValue] = field(default_factory=dict) + schema_version: int = TASK_MOTION_PLAN_SCHEMA_VERSION + + def __post_init__(self) -> None: + for field_name in ("plan_id", "request_digest", "snapshot_digest", "backend", "backend_version"): + _require_text(getattr(self, field_name), field_name) + if isinstance(self.seed, bool) or not isinstance(self.seed, int) or self.seed < 0: + raise ValueError("seed must be a non-negative integer") + if self.schema_version != TASK_MOTION_PLAN_SCHEMA_VERSION: + raise ValueError(f"unsupported task-motion plan schema version {self.schema_version}") + if not self.segments: + raise ValueError("task-motion plan must contain at least one segment") + segment_ids = [segment.segment_id for segment in self.segments] + if len(segment_ids) != len(set(segment_ids)): + raise ValueError("task-motion plan segment IDs must be unique") + known = set(segment_ids) + dependencies: dict[str, set[str]] = {} + for segment in self.segments: + missing = set(segment.depends_on) - known + if missing: + raise ValueError(f"segment {segment.segment_id!r} has unknown dependencies {sorted(missing)}") + if segment.segment_id in segment.depends_on: + raise ValueError(f"segment {segment.segment_id!r} cannot depend on itself") + dependencies[segment.segment_id] = set(segment.depends_on) + if isinstance(segment, ConcurrentGroupSegment): + missing_members = set(segment.member_segment_ids) - known + if missing_members: + raise ValueError( + f"concurrent group {segment.segment_id!r} has unknown members {sorted(missing_members)}" + ) + if segment.segment_id in segment.member_segment_ids: + raise ValueError(f"concurrent group {segment.segment_id!r} cannot contain itself") + _assert_acyclic(dependencies) + metadata = _json_value(self.metadata) + assert isinstance(metadata, dict) + object.__setattr__(self, "metadata", metadata) + + def to_dict(self) -> dict[str, Any]: + return { + "backend": {"name": self.backend, "version": self.backend_version}, + "goal": [predicate.to_dict() for predicate in self.goal], + "metadata": dict(self.metadata), + "plan_id": self.plan_id, + "request_digest": self.request_digest, + "schema_version": self.schema_version, + "seed": self.seed, + "segments": [segment_to_dict(segment) for segment in self.segments], + "snapshot_digest": self.snapshot_digest, + } + + def canonical_json(self) -> str: + """Return deterministic, finite JSON suitable for hashing and run records.""" + + return _canonical_json(self.to_dict()) + + @property + def digest(self) -> str: + return hashlib.sha256(self.canonical_json().encode("utf-8")).hexdigest() + + @classmethod + def from_dict(cls, value: Mapping[str, Any]) -> TaskMotionPlan: + backend = value["backend"] + return cls( + plan_id=value["plan_id"], + request_digest=value["request_digest"], + snapshot_digest=value["snapshot_digest"], + backend=backend["name"], + backend_version=backend["version"], + seed=value["seed"], + segments=tuple(segment_from_dict(segment) for segment in value["segments"]), + goal=tuple(GoalPredicate.from_dict(predicate) for predicate in value.get("goal", ())), + metadata=value.get("metadata", {}), + schema_version=value.get("schema_version", TASK_MOTION_PLAN_SCHEMA_VERSION), + ) + + +def _assert_acyclic(dependencies: Mapping[str, set[str]]) -> None: + visiting: set[str] = set() + visited: set[str] = set() + + def visit(segment_id: str) -> None: + if segment_id in visiting: + raise ValueError(f"task-motion plan dependency cycle includes {segment_id!r}") + if segment_id in visited: + return + visiting.add(segment_id) + for dependency in dependencies[segment_id]: + visit(dependency) + visiting.remove(segment_id) + visited.add(segment_id) + + for segment_id in dependencies: + visit(segment_id) + + +class ExecutionEventType(StrEnum): + PLAN_STARTED = "plan_started" + PLAN_COMPLETED = "plan_completed" + PLAN_FAILED = "plan_failed" + SEGMENT_STARTED = "segment_started" + SEGMENT_COMPLETED = "segment_completed" + SEGMENT_FAILED = "segment_failed" + GRASP_INTENT = "grasp_intent" + GRASP_VERIFIED = "grasp_verified" + GRASP_REJECTED = "grasp_rejected" + TASK_VERIFIED = "task_verified" + TASK_REJECTED = "task_rejected" + RECORDING_COMPLETED = "recording_completed" + RECORDING_FAILED = "recording_failed" + + +class ExecutionOutcome(StrEnum): + PENDING = "pending" + SUCCEEDED = "succeeded" + FAILED = "failed" + SKIPPED = "skipped" + + +@dataclass(frozen=True) +class ExecutionEvent: + """One bounded observation or lifecycle event from plan execution.""" + + event_id: str + attempt_id: str + plan_id: str | None + segment_id: str | None + event_type: ExecutionEventType + outcome: ExecutionOutcome + monotonic_time_s: float + verifier: str | None = None + failure_code: str | None = None + message: str | None = None + metadata: Mapping[str, JsonValue] = field(default_factory=dict) + schema_version: int = EXECUTION_EVENT_SCHEMA_VERSION + + def __post_init__(self) -> None: + _require_text(self.event_id, "event_id") + _require_text(self.attempt_id, "attempt_id") + for field_name in ("plan_id", "segment_id", "verifier", "failure_code"): + value = getattr(self, field_name) + if value is not None: + _require_text(value, field_name) + if self.message is not None: + _require_text(self.message, "message", maximum=2048) + if not isinstance(self.event_type, ExecutionEventType): + object.__setattr__(self, "event_type", ExecutionEventType(self.event_type)) + if not isinstance(self.outcome, ExecutionOutcome): + object.__setattr__(self, "outcome", ExecutionOutcome(self.outcome)) + timestamp = _finite_float(self.monotonic_time_s, "monotonic_time_s") + if timestamp < 0: + raise ValueError("monotonic_time_s must be non-negative") + object.__setattr__(self, "monotonic_time_s", timestamp) + if self.schema_version != EXECUTION_EVENT_SCHEMA_VERSION: + raise ValueError(f"unsupported execution event schema version {self.schema_version}") + metadata = _json_value(self.metadata) + assert isinstance(metadata, dict) + object.__setattr__(self, "metadata", metadata) + + def to_dict(self) -> dict[str, Any]: + return { + "attempt_id": self.attempt_id, + "event_id": self.event_id, + "event_type": self.event_type.value, + "failure_code": self.failure_code, + "message": self.message, + "metadata": dict(self.metadata), + "monotonic_time_s": self.monotonic_time_s, + "outcome": self.outcome.value, + "plan_id": self.plan_id, + "schema_version": self.schema_version, + "segment_id": self.segment_id, + "verifier": self.verifier, + } diff --git a/isaac_autodata_core/schedulestream_algorithm.py b/isaac_autodata_core/schedulestream_algorithm.py new file mode 100644 index 0000000..cd516cc --- /dev/null +++ b/isaac_autodata_core/schedulestream_algorithm.py @@ -0,0 +1,130 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""ScheduleStream generation algorithm (cuStream2 / cuRoboV2 backend). + +Unlike MimicGen/SkillGen — which transform *recorded source demonstrations* per subtask — +ScheduleStream plans the **whole task from scratch** from a symbolic goal via cuStream2's +``solve_tamp`` and replays the resulting command sequence. The planning itself lives in +:class:`~isaac_autodata_core.schedulestream_planner.ScheduleStreamPlanner`, a cuStream2 +:class:`Planner` subclass grounded in the Isaac AutoData env (see that module). + +To fit the per-subtask plug-in contract, the matching task descriptor declares a **single +subtask** per EEF (see ``tasks/franka_cube_stack_schedulestream.yaml``). The single +:meth:`ScheduleStream.plan_subtask_trajectory` call solves TAMP once and returns the entire +trajectory as a flat ``list[Waypoint]``; :class:`DataGenerator` then steps and records it like any +other generated demo. + +The planner module (and its cuRobo / cuStream2 / isaaclab dependency chain) is imported lazily +inside :meth:`ScheduleStream._get_planner`, so importing this module to register the algorithm +stays cheap and dependency-free until a run actually selects ``--alg schedulestream``. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from isaac_autodata_core.algorithms import GenerationAlgorithm + +if TYPE_CHECKING: + from isaac_autodata_core.data_generator import DataGenerator, _EEFGenerationState + from isaac_autodata_core.schedulestream_planner import ScheduleStreamPlanner + from isaac_autodata_core.waypoint import Waypoint + from isaac_autodata_interfaces.datastream.datastream import Datastream + + +class ScheduleStream(GenerationAlgorithm): + """Whole-task TAMP via cuStream2 (cuRoboV2), exposed as a single-subtask generation algorithm. + + The algorithm owns one :class:`ScheduleStreamPlanner` per ``env_id`` (lazily built, reused + across ``generate()`` calls). Each invocation re-syncs the planner world from the live env, + solves TAMP for the task goal, and converts the command rollout into executable waypoints. + """ + + name = "schedulestream" + expected_eef_count = 1 + requires_motion_planner = False + uses_subtask_start_signals = False + supports_coordination = False + + def __init__(self, success_term: Any) -> None: + """ + Args: + success_term: The task's success :class:`TerminationTermCfg`; the symbolic goal is + derived from it (e.g. the cube-stacking termination yields stacking ``Attached`` + relations). It is passed in because ``setup_env_config`` strips it from the env, so + it cannot be recovered at plan time. All other tuning (collisions, max_time, + profile, hold, animate) is read from the task descriptor's single-subtask + ``algo_params`` (:class:`ScheduleStreamSubtaskAlgoParams`) via the datastream — + see :meth:`_get_planner`. + """ + self.success_term = success_term + self._planners: dict[int, ScheduleStreamPlanner] = {} + + def validate_setup(self, datastream: Datastream) -> None: + """Require exactly one subtask per EEF — ScheduleStream plans the whole task at once.""" + for eef_name in datastream.get_eef_names(): + num_subtasks = datastream.num_subtasks(eef_name) + if num_subtasks != 1: + raise ValueError( + "schedulestream plans the whole task in one shot and expects exactly one " + f"subtask per EEF, but EEF {eef_name!r} declares {num_subtasks}. Use a " + "single-subtask descriptor (e.g. tasks/franka_cube_stack_schedulestream.yaml)." + ) + + def plan_subtask_trajectory( + self, + *, + data_generator: DataGenerator, + env_id: int, + eef_name: str, + eef_state: _EEFGenerationState, + all_randomized_subtask_boundaries: dict, + runtime_subtask_constraints_dict: dict, + selected_src_demo_inds: dict, + ) -> tuple[list[Waypoint], bool] | None: + """Solve TAMP once for the (single) subtask and return the full trajectory. + + Returns ``(waypoints, False)`` to execute the planned trajectory as the subtask. + + On planning failure the planner RAISES rather than returning ``None``. Returning ``None`` + makes ``generate()`` report failure without ever enqueuing an action, but ``env_loop`` + blocks waiting for an action and so never reaches its attempt-count stop check — the failed + attempt is retried forever (a livelock of repeated tracebacks). Raising propagates out + through ``generate()`` and the data-gen task, where ``env_loop`` re-raises it and the + process terminates with the traceback. For multi-trial production with retry-on-failure, + this would need the upstream env_loop to count no-action attempts; until then, fail loudly. + """ + planner = self._get_planner(data_generator.datastream, env_id) + waypoints = planner.get_waypoints() + if not waypoints: + raise RuntimeError( + f"schedulestream planning produced no trajectory for env {env_id} " + "(solve_tamp found no plan or returned empty commands)." + ) + return waypoints, False + + def _get_planner(self, datastream: Datastream, env_id: int) -> ScheduleStreamPlanner: + if env_id not in self._planners: + # Deferred import: the planner module pulls isaaclab + cuStream2/cuRobo, which require + # the running Isaac app; importing this module (to register the algorithm) stays cheap. + from isaac_autodata_core.schedulestream_planner import ScheduleStreamPlanner + + # Tuning lives on the single subtask's algo_params (ScheduleStreamSubtaskAlgoParams), + # read here via the datastream so it stays in the task descriptor, not the CLI. + params = datastream.get_subtask_algo_params(datastream.get_eef_names()[0])[0] + self._planners[env_id] = ScheduleStreamPlanner( + datastream, + self.success_term, + env_id=env_id, + metric="ee", + motion="tool", + collisions=params.collisions, + max_time=params.max_time, + profile=params.profile, + hold=params.hold, + animate=params.animate, + ) + return self._planners[env_id] diff --git a/isaac_autodata_core/schedulestream_planner.py b/isaac_autodata_core/schedulestream_planner.py new file mode 100644 index 0000000..44411bd --- /dev/null +++ b/isaac_autodata_core/schedulestream_planner.py @@ -0,0 +1,404 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""ScheduleStream TAMP planner grounded in an Isaac AutoData env. + +Subclasses cuStream2's :class:`Planner`: the observation hooks read the live +IsaacLab scene and ``extract_action`` emits Isaac AutoData :class:`Waypoint`\\ s. + +Import only after the Isaac app is launched (the isaaclab imports require it). +""" + +from __future__ import annotations + +import os +import torch +from typing import TYPE_CHECKING, Any + +# Side effect: patches curobo's USD parser to fan-triangulate quad/n-gon faces; +# Arena/RoboLab assets otherwise fail obstacle extraction. +import schedulestream.applications.robolab.usd_utils # noqa: F401 +from curobo.types import JointState, Pose +from isaaclab.envs import ManagerBasedEnv +from isaaclab.envs.mdp.actions.task_space_actions import DifferentialInverseKinematicsAction +from isaaclab.utils.math import convert_quat +from isaaclab_tasks.manager_based.manipulation.stack.mdp import cubes_stacked +from schedulestream.applications.custream2.animate import process_task_commands +from schedulestream.applications.custream2.command import Attach, Detach, LinkPath +from schedulestream.applications.custream2.franka import load_franka_config +from schedulestream.applications.custream2.policy import Planner +from schedulestream.applications.custream2.scene import CAMERA_POSE +from schedulestream.applications.custream2.tamp import Attached, Holding, movable_from_goal +from schedulestream.applications.custream2.utils import autograd_enabled, multiply_poses, to_cpu +from schedulestream.applications.custream2.world import TAMPConfig, World +from schedulestream.common.utils import apply_mapping, profiler + +from isaac_autodata_core.schedulestream_utils import create_objects, destination_from_contact_sensor +from isaac_autodata_core.waypoint import Waypoint + +if TYPE_CHECKING: + from isaac_autodata_interfaces.datastream.datastream import Datastream + +# Binary gripper action; flip OPEN_ACTION if the env's convention is inverted. +OPEN_ACTION = 1.0 +CLOSE_ACTION = -OPEN_ACTION + + +class ScheduleStreamPlanner(Planner): + """cuStream2 :class:`Planner` grounded in the Isaac AutoData env for one ``env_id``.""" + + def __init__( + self, + datastream: Datastream, + success_term: Any, + *, + env_id: int = 0, + batch_size: int = 128, + collisions: bool = True, + max_time: float = 60.0, + profile: bool = False, + hold: int | None = None, + noise: float = 0.0, + animate: bool = False, + verbose: bool = True, + **kwargs: Any, + ) -> None: + self.datastream = datastream + self.env = datastream.get_env() + self.env_id = env_id + self.profile = profile + self.hold = hold + self.noise = noise + self.verbose = verbose + goal = self.create_goal(success_term) + world = self._create_world(goal, ik_batch=batch_size) + # collisions/profile and extra kwargs flow through **solve_kwargs into solve_tamp. + super().__init__( + world, goal=goal, max_time=max_time, animate=animate, collisions=collisions, profile=profile, **kwargs + ) + # The IK action's control link + body offset are invariant, so resolve them once. + self.body_name, self.body_offset = self._eef_action_info() + # Calibrated per get_waypoints() call, at the synced start configuration. + self._correction: torch.Tensor | None = None + + # ------------------------------------------------------------------ + # Env / scene accessors + # ------------------------------------------------------------------ + + @property + def base_env(self) -> Any: + if isinstance(self.env, ManagerBasedEnv): + return self.env + return self.env.env + + @property + def scene(self) -> Any: + return self.base_env.scene + + @property + def robot(self) -> str: + [robot] = self.scene.articulations + return robot + + @property + def articulation(self) -> Any: + return self.scene.articulations[self.robot] + + # ------------------------------------------------------------------ + # Observation hooks (Planner interface) + # ------------------------------------------------------------------ + + def _to_pose(self, root_pose: Any) -> Pose: + """Convert a batched IsaacLab ``root_pose`` row to a cuRobo Pose (xyzw -> wxyz).""" + root_pose = self.world.to_device(root_pose[self.env_id : self.env_id + 1]) + root_pose[..., 3:7] = convert_quat(root_pose[..., 3:7], to="wxyz") + return Pose(position=root_pose[:, :3], quaternion=root_pose[:, 3:]) + + def get_env_robot_pose(self) -> Pose: + """Return the robot's world root pose.""" + return self._to_pose(self.scene.state["articulation"][self.robot]["root_pose"]) + + def get_env_object_pose(self, obj: str) -> Pose: + """Return the named object's world pose.""" + for body_type, bodies in self.scene.state.items(): + if (body_type == "articulation") or (obj not in bodies): + continue + body = bodies[obj] + if "root_pose" in body: + return self._to_pose(body["root_pose"]) + if "nodal_position" in body: + # Deformables expose nodal state only: use the mean nodal position and keep + # the world model's current orientation (they carry no root orientation). + position = self.world.to_device(body["nodal_position"][self.env_id]).mean(dim=0, keepdim=True) + world_pose = multiply_poses(self.get_env_robot_pose(), self.world.get_object_pose(obj)) + return Pose(position=position, quaternion=world_pose.quaternion) + raise KeyError(f"Object {obj!r} not in the env scene state") + + def get_env_joint_state(self) -> JointState: + """Return the robot's current joint state in world joint order.""" + positions = self.scene.state["articulation"][self.robot]["joint_position"][self.env_id] + mapping = dict(zip(self.articulation.joint_names, to_cpu(positions))) + positions = torch.tensor( + apply_mapping(mapping, self.world.all_joints), dtype=torch.float32, device=self.world.device + ) + return JointState.from_position(positions.unsqueeze(0), joint_names=self.world.all_joints) + + # ------------------------------------------------------------------ + # World construction + # ------------------------------------------------------------------ + + def _create_world(self, goal: Any = None, **kwargs: Any) -> World: + # Only the goal's Attached/Holding objects need to float; an empty set + # falls back to the env's default movability. + objects = create_objects( + self.scene, env_id=self.env_id, floating=movable_from_goal(goal) or None, verbose=self.verbose + ) + + usd_name = os.path.basename(self.articulation.cfg.spawn.usd_path) + + # The Arena stand is part of the robot USD but NOT in cuRobo's collision model + # (custream2's Franka URDF has no stand link). + if usd_name not in ("panda_instanceable.usd", "franka_panda_hand_on_stand.usd"): + raise NotImplementedError( + f"schedulestream currently supports only the Franka panda, got robot USD {usd_name!r}" + ) + robot_config = load_franka_config(base_poses=None) + tamp_config = TAMPConfig() + tamp_config.approach_rest_steps = 50 + # tamp_config.position_velocity /= 2 + # tamp_config.orientation_velocity /= 2 + + # env_loop runs under torch.inference_mode(); building the World there allocates + # cuRobo "inference tensors" that can't be backward()'d (IK) or updated in-place. + # autograd_enabled() makes them normal tensors throughout. + with profiler(field="cumtime" if self.profile else None, num=25), autograd_enabled(): + world = World(robot_config, objects, tamp_config=tamp_config, debug=True, **kwargs) + + # The observation hooks read self.world (Planner.__init__ re-assigns the same World). + self.world = world + self.update_joint_state() + world.set_camera_pose(CAMERA_POSE) + + # Captures cuRobo's CUDA graphs; without it the first solve replays a + # never-captured graph and crashes in seed_ik_solver. + world.warmup() + + return world + + # ------------------------------------------------------------------ + # Goal + # ------------------------------------------------------------------ + + def create_goal(self, success_term: Any) -> Any: + """Derive the symbolic goal from the task success term.""" + if success_term is None: + return self._create_default_goal() + if success_term.func == cubes_stacked: + return self._create_stack_goal(success_term) + arena_goal = self._create_arena_goal(success_term) + if arena_goal is not None: + return arena_goal + raise NotImplementedError(f"schedulestream goal not implemented for {success_term.func}") + + def _create_default_goal(self) -> Any: + """No success term: one movable -> hold it (lift); several -> stack the two highest-indexed.""" + movable = self.world.movable_names + if len(movable) == 1: + # [arm] = self.world.arms + arm = World.ARM + return Holding(arm) <= movable[0] + obj1, obj2 = sorted(movable, reverse=True)[:2] + return Attached(obj1) == obj2 + + def _create_stack_goal(self, success_term: Any) -> Any: + """Goal for isaaclab_tasks' ``cubes_stacked``: cube2 on cube1, then cube3 on cube2.""" + cubes: list[str | None] = [f"cube_{i}" for i in range(1, 3 + 1)] + for i, cube in enumerate(cubes): + cube_cfg = f"{cube}_cfg" + if cube_cfg not in success_term.params: + continue + if success_term.params[cube_cfg] is None: + cubes[i] = None + else: + cubes[i] = success_term.params[cube_cfg].name + cube1, cube2, cube3 = cubes + goal = Attached(cube2) == cube1 + if cube3 is not None: + goal = goal & (Attached(cube3) == cube2) + return goal + + def _create_arena_goal(self, success_term: Any) -> Any: + """Map IsaacLab-Arena success terms to symbolic goals; None if not an Arena term.""" + try: + from isaaclab_arena.tasks import terminations as arena_terminations + except ImportError: + return None + params = success_term.params + + if success_term.func is arena_terminations.object_on_destination: + destination = destination_from_contact_sensor( + self.scene, params["contact_sensor_cfg"].name, env_id=self.env_id + ) + return Attached(params["object_cfg"].name) == destination + + if success_term.func is arena_terminations.objects_on_destinations: + goal = None + for object_cfg, sensor_cfg in zip(params["object_cfg_list"], params["contact_sensor_cfg_list"]): + clause = Attached(object_cfg.name) == destination_from_contact_sensor( + self.scene, sensor_cfg.name, env_id=self.env_id + ) + goal = clause if goal is None else (goal & clause) + return goal + + if success_term.func is arena_terminations.lift_object_il_success: + # [arm] = self.world.arms + arm = World.ARM + return Holding(arm) == params["object_cfg"].name + + return None + + # ------------------------------------------------------------------ + # EEF frame calibration + action extraction + # ------------------------------------------------------------------ + + def _eef_action_info(self) -> tuple[str, Any]: + """Return ``(ik_body_name, body_offset_pose_or_None)`` for the EEF's IK action term.""" + action_manager = getattr(self.base_env, "action_manager", None) + assert action_manager is not None, "env has no action_manager" + for term_name in action_manager.active_terms: + term = action_manager.get_term(term_name) + if isinstance(term, DifferentialInverseKinematicsAction): + offset = None + if term.cfg.body_offset is not None: + offset = Pose( + position=self.world.to_device(list(term.cfg.body_offset.pos)), + quaternion=self.world.to_device(list(term.cfg.body_offset.rot)), + ) + return term.cfg.body_name, offset + raise NotImplementedError("schedulestream requires a DifferentialInverseKinematicsAction (IK env)") + + def _raw_eef_pose(self) -> torch.Tensor: + """World-frame EEF target as cuRobo sees it: ``robot_root * node_pose(body) * body_offset`` + (cuRobo's ``panda_hand`` convention; :meth:`_eef_frame_correction` maps to the env's ee_frame).""" + link_pose = self.world.get_node_pose(self.body_name) + if self.body_offset is not None: + link_pose = link_pose.multiply(self.body_offset) + link_pose = multiply_poses(self.get_env_robot_pose(), link_pose) + return link_pose.get_matrix().squeeze(0).to(device=self.datastream.device, dtype=torch.float32) + + def _eef_frame_correction(self) -> torch.Tensor: + """Constant body-frame transform mapping cuRobo's EEF frame to the env's ee_frame, + measured once at the current config: ``inv(raw_eef) @ obs_eef``. + + custream2's Franka URDF defines ``panda_hand`` ~180 deg about z (and a slightly + different z offset) relative to this IsaacLab version's Franka USD. The mismatch is a + constant link-local transform, so one measurement reproduces the true EEF pose at + every config (obs(q) = raw(q) @ C) — valid for full trajectories, not just holds. + """ + eef_name = self.datastream.get_eef_names()[0] + obs_eef = self.datastream.get_robot_eef_pose(env_ids=[self.env_id], eef_name=eef_name)[0].to( + device=self.datastream.device, dtype=torch.float32 + ) + return torch.linalg.inv(self._raw_eef_pose()) @ obs_eef + + def extract_action(self) -> Waypoint: + """Extract the :class:`Waypoint` (EEF pose target + binary gripper) implied by the world state. + + The gripper closes while the arm holds an attachment. ``noise`` must be a + float, not None: the embodiment adapter does ``noise > 0.0``. + """ + assert self._correction is not None, "get_waypoints() calibrates the frame correction first" + pose = self._raw_eef_pose() @ self._correction + gripper = CLOSE_ACTION if self.get_world_arm_attachments() else OPEN_ACTION + gripper_action = torch.tensor([gripper], dtype=torch.float32, device=self.datastream.device) + return Waypoint(pose=pose, gripper_action=gripper_action, noise=self.noise) + + # ------------------------------------------------------------------ + # Planning + # ------------------------------------------------------------------ + + def plan(self) -> list[Any]: + """Base planning under autograd (cuRobo IK/trajopt call ``backward()``); raise on failure. + + Returning an empty result would livelock ``env_loop``: a failed attempt + enqueues no action, so its attempt-count stop check is never reached. + """ + if self.profile: + print(f"{'=' * 30} PROFILE: solve_tamp {'=' * 30}") + with autograd_enabled(): + commands = super().plan() + if commands is None: + raise RuntimeError(f"solve_tamp found no plan for env {self.env_id}.") + return commands + + def waypoints_from_execution(self, commands: list[Any]) -> list[Waypoint]: + """Roll the plan out open loop in the world model, one Waypoint per step. + + Task-space commands run IK during execute, so this needs autograd. + """ + with autograd_enabled(): + return list(self.get_controller(commands)) + + def _waypoint_from_link_pose(self, link_pose: Pose, gripper: float) -> Waypoint: + """Build the Waypoint for a planned tool-link pose (mirrors extract_action + without reading the world state).""" + if self.body_offset is not None: + link_pose = link_pose.multiply(self.body_offset) + link_pose = multiply_poses(self.get_env_robot_pose(), link_pose) + matrix = link_pose.get_matrix().squeeze(0).to(device=self.datastream.device, dtype=torch.float32) + gripper_action = torch.tensor([gripper], dtype=torch.float32, device=self.datastream.device) + return Waypoint(pose=matrix @ self._correction, gripper_action=gripper_action, noise=self.noise) + + def waypoints_from_commands(self, commands: list[Any]) -> list[Waypoint]: + """Extract Waypoints directly from the plan without executing it. + + Supports only LinkPath (one Waypoint per pose), Attach, and Detach + (gripper flips, each repeating the last pose with the new gripper). + """ + assert self._correction is not None, "get_waypoints() calibrates the frame correction first" + # Convert the whole plan to task space so trajectories become LinkPaths. + commands = process_task_commands(commands, task_space=True) + waypoints: list[Waypoint] = [] + gripper = OPEN_ACTION + last_pose = None + for command in commands: + print(command) + if isinstance(command, LinkPath): + # cuStream's tool frame may be a programmatically added frame that + # differs from the IK action's body link; both are rigid on the hand, + # so map between them with the constant offset from the world model. + tool_from_body = multiply_poses( + self.world.get_node_pose(command.link).inverse(), + self.world.get_node_pose(self.body_name), + ) + for index in range(command.length): + last_pose = multiply_poses(command.pose(index), tool_from_body) + waypoints.append(self._waypoint_from_link_pose(last_pose, gripper)) + elif isinstance(command, (Attach, Detach)): + gripper = CLOSE_ACTION if isinstance(command, Attach) else OPEN_ACTION + if last_pose is not None: + waypoints.append(self._waypoint_from_link_pose(last_pose, gripper)) + # Other commands (Open/HandCommand, Configuration, ...) carry no EEF pose. + return waypoints + + def get_waypoints(self) -> list[Waypoint]: + """Sync the world, plan (or hold), and turn the plan into Waypoints. + + ``execute`` rolls the plan out in the world model; otherwise the + Waypoints are read directly off the commands (LinkPath/Attach/Detach + plans only). + """ + self.update_state() + # Calibrate the cuRobo->ee_frame correction at the synced start configuration. + self._correction = self._eef_frame_correction() + + if self.hold is not None: + # Null plan: hold the current configuration. + commands = self.hold * [self.world.configuration()] + else: + commands = self.plan() + + waypoints = self.waypoints_from_commands(commands) + return waypoints diff --git a/isaac_autodata_core/schedulestream_utils.py b/isaac_autodata_core/schedulestream_utils.py new file mode 100644 index 0000000..7929a4d --- /dev/null +++ b/isaac_autodata_core/schedulestream_utils.py @@ -0,0 +1,191 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Standalone helpers for the schedulestream TAMP planner.""" + +from __future__ import annotations + +import numpy as np +import trimesh +from collections import Counter +from typing import Any + +from curobo._src.util.usd_scene_parser import UsdSceneParser +from curobo.types import Pose +from isaaclab.sim import find_matching_prims +from pxr import Usd, UsdGeom +from schedulestream.applications.custream2.object import GraspConfig, MeshObject +from schedulestream.applications.custream2.utils import ( + multiply_poses, + position_from_pose, + simplify_mesh, + to_matrix, + to_pose, +) + + +def get_name_from_path(scene: Any) -> dict: + """Prim path -> env asset name for the scene's movables (rigid + deformable). + + Deformable assets (e.g. the lift teddy bear) are movable objects too; mapping + their prim paths names world objects by the env asset name (e.g. "object"), + not the USD path. + """ + env_assets = {**dict(scene.rigid_objects), **dict(scene.deformable_objects)} + return { + prim.GetPath().pathString: name + for name, asset in env_assets.items() + for prim in find_matching_prims(asset.cfg.prim_path) + } + + +def destination_from_contact_sensor(scene: Any, contact_sensor_name: str, env_id: int = 0) -> str: + """Resolve an Arena contact sensor's filtered destination prim into a world object name + (the success terms identify the destination only through ``filter_prim_paths_expr``).""" + sensor = scene.sensors[contact_sensor_name] + [filter_expr] = sensor.cfg.filter_prim_paths_expr + path = filter_expr.replace(".*", f"{env_id}") + name_from_path = get_name_from_path(scene) + for prim_path, name in name_from_path.items(): + if path.startswith(prim_path) or prim_path.startswith(path): + return name + raise KeyError( + f"Contact sensor {contact_sensor_name!r} destination {path!r} matches no parsed world " + f"object (known: {sorted(set(name_from_path.values()))})" + ) + + +def prim_relative_pose(stage: Any, prim_path: str, reference_prim_path: str, timecode: float = 0.0) -> Pose: + """The prim's stage xform re-expressed in the reference prim's frame.""" + time = Usd.TimeCode(timecode) + matrix = np.array(UsdGeom.Xformable(stage.GetPrimAtPath(prim_path)).ComputeLocalToWorldTransform(time)).T + reference_matrix = np.array( + UsdGeom.Xformable(stage.GetPrimAtPath(reference_prim_path)).ComputeLocalToWorldTransform(time) + ).T + return to_pose(np.linalg.inv(reference_matrix) @ matrix) + + +def is_degenerate_mesh(mesh: trimesh.Trimesh, name: str = "") -> bool: + """True when the mesh would break sphere fitting (empty, non-finite + vertices, or too few points for a hull -- NaN into cKDTree).""" + if (mesh.vertices.size == 0) or (not np.isfinite(mesh.vertices).all()) or (len(mesh.vertices) < 4): + print(f"[schedulestream] WARNING: skipping obstacle {name!r} with degenerate mesh") + return True + return False + + +# Static obstacles larger than this (max bounding-box extent, metres) or named +# like a floor are dropped from the world: Arena backgrounds (e.g. +# pick_and_place_maple_table) parse the ground plane into a huge mesh that +# swamps collision checking without ever being reachable. +MAX_OBSTACLE_EXTENT = 10.0 +FLOOR_NAME_SUBSTRINGS = ("floor", "ground") + + +def create_objects( + scene: Any, + env_id: int = 0, + floating: set[str] | None = None, + verbose: bool = False, + **simplify_kwargs: Any, +) -> list[Any]: + """Parse the live composed stage into MeshObjects, merging every prim of + the same env asset (rigid or deformable) into one object. + + Follows robolab's ``build_objects_from_usd`` (asset-root grouping: verts + baked into the root prim's frame, object pose set to that root's + robot-relative xform — the frame the env reports, so pose syncs don't + shift the mesh) but parses the live composed stage in the robot + reference frame. Static prim names are de-duplicated with + numeric suffixes. ``simplify_kwargs`` forward to ``simplify_mesh`` for + merged assets. + + ``floating`` overrides which assets are movable: when a set is given, an + asset floats iff its name is in it; when ``None``, rigid objects float + unless kinematic and deformables always float. + """ + usd_parser = UsdSceneParser() + usd_parser.load_stage(scene.stage) + env_path = scene.env_regex_ns.replace(".*", f"{env_id}") + robot_path = f"{env_path}/Robot" + ignore_list = [robot_path, f"{env_path}/target", "/World/defaultGroundPlane", "/curobo"] + scene_cfg = usd_parser.get_obstacles_from_stage( + only_paths=[env_path], + reference_prim_path=robot_path, + ignore_substring=ignore_list, + timecode=0, + ) + assert scene_cfg.objects, f"no obstacles parsed under {env_path}" + stage = usd_parser.stage + + # Prim path -> env asset name for movables (rigid + deformable). + name_from_path = get_name_from_path(scene) + + groups: dict[str, list[Any]] = {} + roots: dict[str, str] = {} + static_obstacles: list[Any] = [] + for obstacle in scene_cfg.objects: + for prim_path, name in name_from_path.items(): + if obstacle.name.startswith(prim_path): + groups.setdefault(name, []).append(obstacle) + roots.setdefault(name, prim_path) + break + else: + static_obstacles.append(obstacle) + + # Asset names are reserved; repeated static names get _1, _2, ... suffixes. + name_counts = Counter(groups.keys()) + + objects = [] + # Unmatched prims (background scenery, tables, fixtures) are static + # collision-only obstacles. + for obstacle in static_obstacles: + mesh = obstacle.get_trimesh_mesh() + if is_degenerate_mesh(mesh, obstacle.name): + continue + extent = float(max(mesh.extents)) + if (extent > MAX_OBSTACLE_EXTENT) or any(part in obstacle.name.lower() for part in FLOOR_NAME_SUBSTRINGS): + print(f"[schedulestream] skipping floor/oversized obstacle {obstacle.name!r} (extent {extent:.1f} m)") + continue + base = obstacle.name.replace("/", "_").lstrip("_") + name_counts[base] += 1 + name = base if name_counts[base] == 1 else f"{base}_{name_counts[base] - 1}" + objects.append(MeshObject(name, mesh, pose=to_pose(obstacle.pose), surface_config=None)) + + for name, obstacles in groups.items(): + if len(obstacles) > 1: + print(f"[schedulestream] merging {name!r} from {len(obstacles)} prims") + root_pose = prim_relative_pose(stage, roots[name], robot_path) + root_inv = root_pose.inverse() + baked = [] + for obstacle in obstacles: + mesh = obstacle.get_trimesh_mesh().copy() + if is_degenerate_mesh(mesh, obstacle.name): + continue + mesh.apply_transform(to_matrix(multiply_poses(root_inv, to_pose(obstacle.pose)))) + baked.append(mesh) + if not baked: + continue + mesh = baked[0] if len(baked) == 1 else trimesh.util.concatenate(baked) + mesh = simplify_mesh(mesh, **simplify_kwargs) + # Movability mirrors _convert_objects: matched rigid objects unless + # kinematic; matched deformables always. + if floating is not None: + is_floating = name in floating + elif name in scene.rigid_objects: + spawn = scene.rigid_objects[name].cfg.spawn + is_floating = not ( + (spawn is not None) and (spawn.rigid_props is not None) and spawn.rigid_props.kinematic_enabled + ) + else: + is_floating = True + grasp_config = GraspConfig() if is_floating else None + objects.append(MeshObject(name, mesh, pose=root_pose, grasp_config=grasp_config, surface_config=None)) + if verbose: + print( + f"[schedulestream] Object: {name} | Prims: {len(obstacles)} | Floating: {is_floating}" + f" | Position: {np.round(position_from_pose(root_pose), 2)}" + ) + return objects diff --git a/isaac_autodata_core/waypoint.py b/isaac_autodata_core/waypoint.py index a15794f..44a787a 100644 --- a/isaac_autodata_core/waypoint.py +++ b/isaac_autodata_core/waypoint.py @@ -320,4 +320,7 @@ async def execute( await env_action_queue.put((env_id, play_action[0])) await env_action_queue.join() + # Envs without a success termination cannot be scored; report not-succeeded. + if success_term is None: + return False return bool(success_term.func(env, **success_term.params)[env_id]) diff --git a/isaac_autodata_examples/autonomous/franka_pick_cube_into_bowl.yaml b/isaac_autodata_examples/autonomous/franka_pick_cube_into_bowl.yaml new file mode 100644 index 0000000..cb89183 --- /dev/null +++ b/isaac_autodata_examples/autonomous/franka_pick_cube_into_bowl.yaml @@ -0,0 +1,66 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# AutoData owns this execution envelope. The nested `environment.intent` mapping is passed without +# reinterpretation to Arena's EnvironmentIntentSpec and IntentCompiler before Isaac is launched. +schema_version: 1 +name: franka_pick_cube_into_bowl + +environment: + intent: + reasoning: >- + Use Arena's registered Franka IK embodiment on the Maple table. Pick the registered Rubik's + cube object and place it into the registered YCB bowl. + background: maple_table_robolab + embodiment: franka_ik + items: + - query: rubiks_cube_hot3d_robolab + category_tags: [object] + instance_name: pick_cube + - query: bowl_ycb_robolab + category_tags: [object, bowl] + instance_name: destination_bowl + initial_state_graph: + - kind: is_anchor + subject: maple_table_robolab + params: {} + - kind: on + subject: pick_cube + reference: maple_table_robolab + params: {} + - kind: on + subject: destination_bowl + reference: maple_table_robolab + params: {} + tasks: + - kind: PickAndPlaceTask + params: + pick_up_object: pick_cube + destination_location: destination_bowl + background_scene: maple_table_robolab + description: Pick up the cube and place it into the bowl. + +planner: + backend: schedulestream + # `auto` selects custream on the current cuRobo-v1 image and custream2 on a reviewed v2 image. + motion_backend: auto + collisions: true + max_time_s: 60.0 + batch_size: 128 + # Arena's Franka IK environment advances one action every 4 x 0.005 s physics steps. + interpolation_dt_s: 0.02 + profile: false + animate: false + +generation: + successful_episodes: 10 + seed: 1 + num_envs: 1 + max_attempts: 50 + +output: + dataset: outputs/franka_pick_cube_into_bowl.hdf5 + keep_failed: false + run_log: outputs/franka_pick_cube_into_bowl_run_log.jsonl diff --git a/isaac_autodata_examples/compile_task_request.py b/isaac_autodata_examples/compile_task_request.py new file mode 100755 index 0000000..a64392d --- /dev/null +++ b/isaac_autodata_examples/compile_task_request.py @@ -0,0 +1,618 @@ +#!/usr/bin/env python +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Compile and preflight an AutoData task request without launching simulation. + +This entrypoint is deliberately a product boundary, not a simulator launcher. It turns one +human-authored semantic YAML request into a deterministic compiled task request, optionally verifies +that the current Python runtime contains a compatible cuRobo/ScheduleStream pair, and then stops. +The later execution lane can consume the compiled task without silently changing user intent. +""" + +from __future__ import annotations + +import argparse +import contextlib +import errno +import json +import os +import secrets +import sys +from collections.abc import Callable, Mapping, Sequence +from dataclasses import dataclass +from enum import IntEnum +from pathlib import Path, PureWindowsPath +from typing import Any, TextIO + +MAX_ARTIFACT_PATH_LENGTH = 4096 +_FORBIDDEN_ARTIFACT_DIRECTORIES = frozenset({".git", ".hg", ".svn"}) +_DIRECTORY_OPEN_FLAGS = os.O_RDONLY | getattr(os, "O_DIRECTORY", 0) | getattr(os, "O_NOFOLLOW", 0) +_FILE_CREATE_FLAGS = os.O_WRONLY | os.O_CREAT | os.O_EXCL | getattr(os, "O_NOFOLLOW", 0) + +# Direct ``python path/to/script.py`` execution puts only the examples directory on sys.path. Keep +# the documented repository-local invocation working without requiring an editable installation. +if __package__ in (None, ""): + _REPOSITORY_ROOT = str(Path(__file__).resolve().parents[1]) + if _REPOSITORY_ROOT not in sys.path: + sys.path.insert(0, _REPOSITORY_ROOT) + + +class ExitCode(IntEnum): + """Stable process exit codes exposed by the task compilation boundary.""" + + SUCCESS = 0 + USAGE = 2 + REQUEST_COMPILATION_FAILED = 3 + RUNTIME_PREFLIGHT_FAILED = 4 + ARTIFACT_WRITE_FAILED = 5 + INTERNAL_ERROR = 6 + + +@dataclass(frozen=True) +class RuntimePreflightReport: + """Successful ScheduleStream runtime selection for one compiled task request.""" + + requested_motion_backend: str + selected_motion_backend: str + schedulestream_application: str + capabilities: Mapping[str, Any] + + def to_dict(self) -> dict[str, Any]: + """Return a JSON-compatible preflight report.""" + + return { + "capabilities": dict(self.capabilities), + "requested_motion_backend": self.requested_motion_backend, + "schedulestream_application": self.schedulestream_application, + "selected_motion_backend": self.selected_motion_backend, + "status": "passed", + } + + +class CompiledTaskWriteError(RuntimeError): + """A safe, stable failure raised while validating or writing an artifact.""" + + def __init__(self, code: str, message: str) -> None: + self.code = code + super().__init__(message) + + +def build_argument_parser() -> argparse.ArgumentParser: + """Build the command-line parser without importing Isaac, Arena, or planner packages.""" + + parser = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=( + "This command never launches Isaac Sim and never generates a dataset.\n\n" + "Exit codes:\n" + " 0 request compiled and requested checks passed\n" + " 2 command-line usage error\n" + " 3 request validation or Arena compilation failed\n" + " 4 cuRobo/ScheduleStream runtime preflight failed\n" + " 5 compiled task path validation or write failed\n" + " 6 unexpected internal error" + ), + ) + parser.add_argument("request", type=Path, help="Path to an AutoData v1 semantic request YAML file.") + parser.add_argument( + "--dry-run", + action="store_true", + help="Compile and resolve the request, but skip the installed-runtime capability probe.", + ) + parser.add_argument( + "--format", + choices=("human", "json"), + default="human", + dest="output_format", + help="Result and error output format (default: human).", + ) + parser.add_argument( + "--json", + action="store_const", + const="json", + dest="output_format", + help="Shorthand for --format json.", + ) + parser.add_argument( + "--write-compiled", + metavar="RELATIVE.json", + help=( + "Write canonical resolved JSON beneath the request file's directory. Existing files, " + "absolute paths, traversal, symlinks, and repository-control directories are rejected." + ), + ) + return parser + + +def run_cli( + argv: Sequence[str] | None = None, + *, + compiler: Callable[[Path], Any] | None = None, + capability_detector: Callable[[], Any] | None = None, + backend_selector: Callable[[str, Any], Any] | None = None, + stdout: TextIO | None = None, + stderr: TextIO | None = None, +) -> int: + """Compile and preflight one request, returning a stable process exit code. + + Dependency injection keeps contract tests independent of Arena and installed motion stacks. + Default dependencies are imported only after argument parsing, and the capability probe is + imported only after semantic compilation succeeds. + + Args: + argv: Arguments excluding the executable name. + compiler: Optional semantic request compiler test seam. + capability_detector: Optional import-free capability detector test seam. + backend_selector: Optional backend selection test seam. + stdout: Success output stream. + stderr: Error output stream. + + Returns: + A value from :class:`ExitCode`. + """ + + args = build_argument_parser().parse_args(argv) + stdout = stdout or sys.stdout + stderr = stderr or sys.stderr + + from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError + + if compiler is None: + from isaac_autodata_interfaces.autonomous.task_compiler import compile_task_request + + compiler = compile_task_request + + try: + resolved = compiler(args.request) + except AutonomousValidationError as exc: + _emit_error( + stderr, + args.output_format, + category="request_compilation", + code="request_invalid", + exit_code=ExitCode.REQUEST_COMPILATION_FAILED, + message="The semantic request could not be compiled.", + details={"issues": [issue.to_dict() for issue in exc.issues]}, + ) + return int(ExitCode.REQUEST_COMPILATION_FAILED) + except Exception as exc: + _emit_unexpected_error(stderr, args.output_format, "request_compilation", exc) + return int(ExitCode.INTERNAL_ERROR) + + preflight: RuntimePreflightReport | None = None + if not args.dry_run: + from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + BackendCompatibilityError, + detect_curobo_runtime, + select_schedulestream_backend, + ) + + capability_detector = capability_detector or detect_curobo_runtime + backend_selector = backend_selector or select_schedulestream_backend + requested_motion_backend = resolved.planner.motion_backend.value + capabilities: Any | None = None + try: + capabilities = capability_detector() + selection = backend_selector(requested_motion_backend, capabilities) + preflight = RuntimePreflightReport( + requested_motion_backend=requested_motion_backend, + selected_motion_backend=selection.motion_backend, + schedulestream_application=selection.schedulestream_application, + capabilities=selection.capabilities.to_dict(), + ) + except BackendCompatibilityError as exc: + _emit_error( + stderr, + args.output_format, + category="runtime_preflight", + code="backend_incompatible", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message=_safe_exception_message(exc), + details={ + "capabilities": _capabilities_to_dict(capabilities), + "requested_motion_backend": requested_motion_backend, + "remediation": ( + "Use motion_backend: auto with a reviewed pinned runtime, or select the " + "development image matching the requested cuRobo/ScheduleStream generation." + ), + }, + ) + return int(ExitCode.RUNTIME_PREFLIGHT_FAILED) + except Exception as exc: + _emit_error( + stderr, + args.output_format, + category="runtime_preflight", + code="runtime_probe_failed", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message=( + f"Runtime capability preflight failed with {type(exc).__name__}: {_safe_exception_message(exc)}" + ), + details={ + "capabilities": _capabilities_to_dict(capabilities), + "requested_motion_backend": requested_motion_backend, + "remediation": "Inspect the pinned runtime image without modifying the AutoData host environment.", + }, + ) + return int(ExitCode.RUNTIME_PREFLIGHT_FAILED) + + artifact_path: Path | None = None + if args.write_compiled is not None: + request_directory = args.request.expanduser().resolve(strict=False).parent + try: + artifact_path = _write_compiled_artifact( + request_directory, + args.write_compiled, + resolved.canonical_json(), + ) + except CompiledTaskWriteError as exc: + _emit_error( + stderr, + args.output_format, + category="compiled_task", + code=exc.code, + exit_code=ExitCode.ARTIFACT_WRITE_FAILED, + message=str(exc), + details={"request_directory": str(request_directory)}, + ) + return int(ExitCode.ARTIFACT_WRITE_FAILED) + except Exception as exc: + _emit_unexpected_error(stderr, args.output_format, "compiled_task", exc) + return int(ExitCode.INTERNAL_ERROR) + + result = _build_success_result(resolved, preflight, artifact_path, dry_run=args.dry_run) + if args.output_format == "json": + _write_json(stdout, result) + else: + _write_human_result(stdout, resolved, preflight, artifact_path, dry_run=args.dry_run) + return int(ExitCode.SUCCESS) + + +def main(argv: Sequence[str] | None = None) -> int: + """Run the task compilation CLI.""" + + return run_cli(argv) + + +def _build_success_result( + resolved: Any, + preflight: RuntimePreflightReport | None, + artifact_path: Path | None, + *, + dry_run: bool, +) -> dict[str, Any]: + if dry_run: + runtime_preflight: dict[str, Any] = { + "reason": "dry_run_requested", + "status": "skipped", + } + else: + assert preflight is not None + runtime_preflight = preflight.to_dict() + return { + "execution": {"dataset_generated": False, "simulation_launched": False}, + "mode": "dry_run" if dry_run else "runtime_preflight", + "compiled_task": resolved.to_dict(), + "compiled_task_path": None if artifact_path is None else str(artifact_path), + "compiled_task_digest": resolved.digest, + "runtime_preflight": runtime_preflight, + } + + +def _write_human_result( + stream: TextIO, + resolved: Any, + preflight: RuntimePreflightReport | None, + artifact_path: Path | None, + *, + dry_run: bool, +) -> None: + generation = resolved.generation + output = resolved.output + lines = [ + "Task request compiled successfully.", + f" Name: {_quoted(resolved.name)}", + f" Environment: {_quoted(resolved.environment_name)}", + f" Request digest: {resolved.request_digest}", + f" Compiled task digest: {resolved.digest}", + f" Arena graph digest: {resolved.graph_digest}", + f" Planner: {resolved.planner.backend.value}", + f" Requested motion backend: {resolved.planner.motion_backend.value}", + f" Goal stages: {len(resolved.goal_stages)}", + ( + " Generation: " + f"successful_episodes={generation.successful_episodes}, max_attempts={generation.max_attempts}, " + f"num_envs={generation.num_envs}, seed={generation.seed}" + ), + f" Dataset path: {_quoted(str(output.dataset))}", + f" RunLog path: {_quoted(None if output.run_log is None else str(output.run_log))}", + ] + if dry_run: + lines.append(" Runtime preflight: skipped (--dry-run)") + else: + assert preflight is not None + lines.extend([ + " Runtime preflight: passed", + f" Selected motion backend: {preflight.selected_motion_backend}", + f" ScheduleStream application: {preflight.schedulestream_application}", + ]) + if artifact_path is not None: + lines.append(f" Compiled task: {_quoted(str(artifact_path))}") + lines.extend([ + "Simulation launched: no.", + "Dataset generated: no.", + ]) + stream.write("\n".join(lines) + "\n") + stream.flush() + + +def _write_compiled_artifact(request_directory: Path, relative_name: str, canonical_json: str) -> Path: + parts = _validate_artifact_relative_path(relative_name) + root = request_directory.resolve(strict=False) + try: + parent_fd = _open_artifact_parent(root, parts[:-1]) + except CompiledTaskWriteError: + raise + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_parent_unavailable", + f"Could not open the request directory securely: {_safe_os_error(exc)}", + ) from None + + target_name = parts[-1] + temporary_name: str | None = None + try: + temporary_name, temporary_fd = _create_temporary_artifact(parent_fd) + try: + try: + _write_all(temporary_fd, (canonical_json + "\n").encode("utf-8")) + os.fsync(temporary_fd) + except CompiledTaskWriteError: + raise + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_write_failed", + f"Could not write the compiled task: {_safe_os_error(exc)}", + ) from None + finally: + with contextlib.suppress(OSError): + os.close(temporary_fd) + try: + os.link( + temporary_name, + target_name, + src_dir_fd=parent_fd, + dst_dir_fd=parent_fd, + follow_symlinks=False, + ) + except FileExistsError: + raise CompiledTaskWriteError( + "artifact_exists", + f"Refusing to overwrite existing compiled task {_quoted(relative_name)}.", + ) from None + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_write_failed", + f"Could not publish the compiled task: {_safe_os_error(exc)}", + ) from None + try: + os.fsync(parent_fd) + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_write_failed", + f"The compiled task was published, but its directory could not be synchronized: {_safe_os_error(exc)}", + ) from None + finally: + active_error = sys.exc_info()[0] is not None + cleanup_error: OSError | None = None + if temporary_name is not None: + try: + os.unlink(temporary_name, dir_fd=parent_fd) + except OSError as exc: + if exc.errno != errno.ENOENT: + cleanup_error = exc + try: + os.close(parent_fd) + except OSError as exc: + cleanup_error = cleanup_error or exc + if cleanup_error is not None and not active_error: + raise CompiledTaskWriteError( + "artifact_write_failed", + f"Could not clean up the compiled task transaction: {_safe_os_error(cleanup_error)}", + ) from None + return root.joinpath(*parts) + + +def _validate_artifact_relative_path(raw_path: str) -> tuple[str, ...]: + if not raw_path: + raise CompiledTaskWriteError("artifact_path_invalid", "Compiled task path must not be empty.") + if len(raw_path) > MAX_ARTIFACT_PATH_LENGTH: + raise CompiledTaskWriteError( + "artifact_path_invalid", + f"Compiled task path exceeds {MAX_ARTIFACT_PATH_LENGTH} characters.", + ) + if any(ord(character) < 32 for character in raw_path): + raise CompiledTaskWriteError("artifact_path_invalid", "Compiled task path contains control characters.") + if "\\" in raw_path: + raise CompiledTaskWriteError( + "artifact_path_invalid", + "Compiled task path must use POSIX '/' separators.", + ) + path = Path(raw_path) + windows_path = PureWindowsPath(raw_path) + if path.is_absolute() or windows_path.is_absolute() or windows_path.drive: + raise CompiledTaskWriteError("artifact_path_absolute", "Compiled task path must be relative.") + if raw_path.startswith("~"): + raise CompiledTaskWriteError("artifact_path_invalid", "Home-directory expansion is not allowed.") + raw_parts = tuple(raw_path.split("/")) + if any(part in ("", ".", "..") for part in raw_parts): + raise CompiledTaskWriteError( + "artifact_path_traversal", + "Compiled task path must not contain empty, '.' or '..' components.", + ) + if any(part in _FORBIDDEN_ARTIFACT_DIRECTORIES for part in raw_parts): + raise CompiledTaskWriteError( + "artifact_path_forbidden", + "Compiled tasks cannot be written inside repository-control directories.", + ) + if Path(raw_parts[-1]).suffix != ".json": + raise CompiledTaskWriteError("artifact_extension_invalid", "Compiled task path must end in '.json'.") + return raw_parts + + +def _open_artifact_parent(root: Path, directory_parts: tuple[str, ...]) -> int: + try: + current_fd = os.open(root, _DIRECTORY_OPEN_FLAGS) + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_parent_unavailable", + f"Could not securely open request directory {_quoted(str(root))}: {_safe_os_error(exc)}", + ) from None + try: + for part in directory_parts: + try: + next_fd = os.open(part, _DIRECTORY_OPEN_FLAGS, dir_fd=current_fd) + except FileNotFoundError: + try: + os.mkdir(part, mode=0o700, dir_fd=current_fd) + next_fd = os.open(part, _DIRECTORY_OPEN_FLAGS, dir_fd=current_fd) + except FileExistsError: + raise CompiledTaskWriteError( + "artifact_path_unsafe", + f"Artifact directory component {_quoted(part)} changed while it was being created.", + ) from None + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_parent_unavailable", + f"Could not create artifact directory component {_quoted(part)}: {_safe_os_error(exc)}", + ) from None + except OSError as exc: + code = ( + "artifact_path_unsafe" + if exc.errno in (errno.ELOOP, errno.ENOTDIR) + else "artifact_parent_unavailable" + ) + raise CompiledTaskWriteError( + code, + f"Could not securely traverse artifact directory component {_quoted(part)}: {_safe_os_error(exc)}", + ) from None + os.close(current_fd) + current_fd = next_fd + return current_fd + except Exception: + os.close(current_fd) + raise + + +def _create_temporary_artifact(parent_fd: int) -> tuple[str, int]: + for _ in range(128): + name = f".autodata-compiled-{secrets.token_hex(12)}.tmp" + try: + file_fd = os.open(name, _FILE_CREATE_FLAGS, 0o600, dir_fd=parent_fd) + except FileExistsError: + continue + except OSError as exc: + raise CompiledTaskWriteError( + "artifact_write_failed", + f"Could not create a temporary compiled task: {_safe_os_error(exc)}", + ) from None + return name, file_fd + raise CompiledTaskWriteError( + "artifact_write_failed", + "Could not allocate a unique temporary compiled task name.", + ) + + +def _write_all(file_descriptor: int, content: bytes) -> None: + offset = 0 + while offset < len(content): + written = os.write(file_descriptor, content[offset:]) + if written == 0: + raise CompiledTaskWriteError("artifact_write_failed", "Compiled task write made no progress.") + offset += written + + +def _capabilities_to_dict(capabilities: Any | None) -> dict[str, Any] | None: + if capabilities is None: + return None + try: + value = capabilities.to_dict() + except Exception: + return {"summary": f"unavailable ({type(capabilities).__name__})"} + return value if isinstance(value, dict) else {"summary": f"invalid ({type(value).__name__})"} + + +def _emit_unexpected_error(stream: TextIO, output_format: str, category: str, exc: Exception) -> None: + _emit_error( + stream, + output_format, + category=category, + code="internal_error", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"Unexpected {type(exc).__name__}: {_safe_exception_message(exc)}", + details={"remediation": "Re-run with validated inputs and report this bounded error to AutoData maintainers."}, + ) + + +def _emit_error( + stream: TextIO, + output_format: str, + *, + category: str, + code: str, + exit_code: ExitCode, + message: str, + details: Mapping[str, Any], +) -> None: + error = { + "category": category, + "code": code, + "details": dict(details), + "exit_code": int(exit_code), + "message": message, + } + if output_format == "json": + _write_json(stream, {"error": error}) + return + stream.write(f"ERROR [{category}/{code}] (exit {int(exit_code)}): {message}\n") + issues = details.get("issues") + if isinstance(issues, list): + for issue in issues: + if isinstance(issue, dict): + stream.write( + f" - {issue.get('path', '$')} [{issue.get('code', 'invalid')}]: {issue.get('message', '')}\n" + ) + remediation = details.get("remediation") + if isinstance(remediation, str): + stream.write(f" Remediation: {remediation}\n") + capabilities = details.get("capabilities") + if capabilities is not None: + stream.write(f" Detected capabilities: {json.dumps(capabilities, sort_keys=True, ensure_ascii=False)}\n") + stream.flush() + + +def _write_json(stream: TextIO, value: Mapping[str, Any]) -> None: + stream.write(json.dumps(value, indent=2, sort_keys=True, ensure_ascii=False, allow_nan=False) + "\n") + stream.flush() + + +def _safe_exception_message(exc: Exception) -> str: + message = " ".join(str(exc).splitlines()).strip() + return (message or "no additional details")[:2048] + + +def _safe_os_error(exc: OSError) -> str: + message = exc.strerror or type(exc).__name__ + return " ".join(message.splitlines())[:512] + + +def _quoted(value: Any) -> str: + return json.dumps(value, ensure_ascii=False, allow_nan=False) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/isaac_autodata_examples/generate_dataset.py b/isaac_autodata_examples/generate_dataset.py index 81e5c09..5e3914d 100755 --- a/isaac_autodata_examples/generate_dataset.py +++ b/isaac_autodata_examples/generate_dataset.py @@ -25,6 +25,10 @@ * ``skillgen`` — single-arm SkillGen. SkillGen depends on a motion-planner interface; until the planner code is ported into this repo, the CLI satisfies that interface with the upstream Arena ``CuroboPlanner``. +* ``schedulestream`` — whole-task TAMP via cuStream2 (cuRoboV2). Instead of transforming source + demos, it solves the task from scratch (``solve_tamp``) from a goal derived from the success + term and replays the plan. Use the single-subtask descriptor + ``tasks/franka_cube_stack_schedulestream.yaml``. The CLI composes a :class:`Datastream` from the task descriptor YAML, the embodiment YAML, the live env, and the HDF5 source dataset, then hands it to :class:`DataGenerator`. @@ -38,7 +42,7 @@ # Hardcoded to keep argparse importable without pulling in the heavy core package. # Add new algorithms here when registering them in isaac_autodata_core.algorithms. -_ALG_CHOICES = ["mimicgen", "dexmimicgen", "skillgen"] +_ALG_CHOICES = ["mimicgen", "dexmimicgen", "skillgen", "schedulestream"] parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("--task", type=str, default=None, help="Name of the task.") @@ -76,6 +80,11 @@ help="Pause after every subtask for interactive debugging.", ) +# ScheduleStream tuning/debug knobs (batch, scale_dt, collisions, max_time, profile, hold, animate) +# live in the task descriptor under the single subtask's `algo_params:` (parsed into +# ScheduleStreamSubtaskAlgoParams), so they stay out of this shared CLI. See +# tasks/franka_cube_stack_schedulestream.yaml. + AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() @@ -93,6 +102,9 @@ import traceback # noqa: E402 from typing import Any # noqa: E402 +# Importing this module self-registers the "schedulestream" algorithm (heavy cuStream2 / cuRobo +# deps inside it are imported lazily, so this import stays cheap until a run selects it). +import isaac_autodata_core.schedulestream_algorithm # noqa: E402,F401 from isaac_autodata_core import DataGenerator, get_algorithm # noqa: E402 from isaac_autodata_core.algorithms import REGISTERED_ALGORITHMS # noqa: E402 from isaac_autodata_interfaces.datastream import Datastream # noqa: E402 @@ -284,12 +296,16 @@ def main() -> None: embodiment_yaml=args_cli.embodiment, ) - # SkillGen needs one curobo planner per env. Mimic/DexMimic take no kwargs. + # SkillGen needs one curobo planner per env. ScheduleStream needs the success term (stripped + # from the env by setup_env_config, so only available here); its other tuning lives in the task + # descriptor's subtask algo_params. Mimic/DexMimic take no kwargs. motion_planners: dict | None = None alg_kwargs: dict = {} if args_cli.alg == "skillgen": motion_planners = _build_motion_planners(datastream, args_cli.num_envs, env_name) alg_kwargs["motion_planners"] = motion_planners + elif args_cli.alg == "schedulestream": + alg_kwargs["success_term"] = success_term algorithm = get_algorithm(args_cli.alg, **alg_kwargs) try: diff --git a/isaac_autodata_examples/generate_task_dataset.py b/isaac_autodata_examples/generate_task_dataset.py new file mode 100755 index 0000000..5cbe7a7 --- /dev/null +++ b/isaac_autodata_examples/generate_task_dataset.py @@ -0,0 +1,1950 @@ +#!/usr/bin/env python +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Run bounded source-free dataset generation from one AutoData task request. + +Semantic compilation and the import-free cuRobo/ScheduleStream capability preflight finish in a +parent process before Isaac AppLauncher is imported in a fresh child. The child launches Isaac +first, recompiles and digest-attests the request, then constructs the linked Arena environment, +creates the selected ScheduleStream planner, and executes bounded attempts. A private pipe carries +terminal status before SimulationApp shutdown, whose process exit code is not trusted. +""" + +from __future__ import annotations + +import argparse +import asyncio +import json +import os +import sys +from collections.abc import Awaitable, Callable, Mapping, Sequence +from contextlib import suppress +from dataclasses import dataclass +from enum import IntEnum +from pathlib import Path +from typing import Any, TextIO + +_RUNTIME_STATUS_PROTOCOL_VERSION = 1 +_MAX_RUNTIME_STATUS_BYTES = 512 + +# The admitted live profile permits up to 50 attempts, with a planner budget of up to 60 seconds +# per solve and several solves per attempt. Six hours leaves conservative simulator/controller +# headroom while still giving the parent a hard upper bound. Shutdown gets a separate short grace +# period before the complete child process group is killed. +_RUNTIME_CHILD_WALL_TIMEOUT_S = 6 * 60 * 60 +_RUNTIME_CHILD_TERMINATE_GRACE_S = 30.0 +_RUNTIME_CHILD_WAIT_POLL_S = 0.25 + +# Direct script execution puts only the examples directory on sys.path. Keep the documented +# repository-local invocation working without requiring an editable installation. +if __package__ in (None, ""): + _REPOSITORY_ROOT = str(Path(__file__).resolve().parents[1]) + if _REPOSITORY_ROOT not in sys.path: + sys.path.insert(0, _REPOSITORY_ROOT) + + +class ExitCode(IntEnum): + """Stable process exits exposed by the dataset generation boundary.""" + + SUCCESS = 0 + USAGE = 2 + REQUEST_COMPILATION_FAILED = 3 + RUNTIME_PREFLIGHT_FAILED = 4 + OUTPUT_CONFLICT = 5 + APP_LAUNCH_FAILED = 6 + RUNTIME_SETUP_FAILED = 7 + GENERATION_INCOMPLETE = 8 + CLEANUP_FAILED = 9 + INTERNAL_ERROR = 10 + INTERRUPTED = 130 + + +@dataclass(frozen=True) +class RuntimeStack: + """Heavy runtime callables loaded only after SimulationApp starts.""" + + output_transaction_factory: Callable[..., Any] + arena_runtime_builder: Callable[..., Any] + goal_projector: Callable[[Any], tuple[Any, ...]] + attachment_state_factory: Callable[[], Any] + runtime_factory: Callable[..., Any] + success_verifier_factory: Callable[[Any], Any] + executor_factory: Callable[..., Any] + planner_factory: Callable[..., Any] + run_log_writer_factory: Callable[..., Any] + generator_factory: Callable[..., Any] + generation_request_factory: Callable[..., Any] + run_loop: Callable[..., Any] + + +@dataclass(frozen=True) +class _RuntimeChildProcessResult: + """Raw status-channel result returned by the real subprocess runner.""" + + process_return_code: int + status_payload: bytes + + +@dataclass(frozen=True) +class CleanupIssue: + """One bounded failure from an owned resource closer.""" + + resource: str + exception_type: str + message: str + + def to_dict(self) -> dict[str, str]: + """Return a safe JSON-compatible cleanup issue.""" + + return { + "exception_type": self.exception_type, + "message": self.message, + "resource": self.resource, + } + + +class _CleanupStack: + """Close acquired resources in reverse order while attempting every callback.""" + + def __init__(self) -> None: + self._callbacks: list[tuple[str, Callable[[], None]]] = [] + + def push(self, resource: str, callback: Callable[[], None]) -> None: + if not callable(callback): + raise TypeError(f"{resource} close callback must be callable") + self._callbacks.append((resource, callback)) + + def close(self) -> tuple[CleanupIssue, ...]: + issues: list[CleanupIssue] = [] + while self._callbacks: + resource, callback = self._callbacks.pop() + try: + callback() + except Exception as exc: + issues.append( + CleanupIssue( + resource=resource, + exception_type=type(exc).__name__, + message=_safe_exception_message(exc), + ) + ) + return tuple(issues) + + +@dataclass(frozen=True) +class CliFailure: + """Structured terminal failure emitted by the process boundary.""" + + category: str + code: str + exit_code: ExitCode + message: str + details: Mapping[str, Any] + + def with_cleanup(self, issues: tuple[CleanupIssue, ...]) -> CliFailure: + details = dict(self.details) + details["cleanup_failures"] = [issue.to_dict() for issue in issues] + return CliFailure( + category=self.category, + code=self.code, + exit_code=self.exit_code, + message=self.message, + details=details, + ) + + +class _CliAbort(Exception): + """Carry one already-classified child failure through the cleanup boundary.""" + + def __init__(self, failure: CliFailure) -> None: + self.failure = failure + super().__init__(failure.message) + + +class _RuntimeChildStatusError(ValueError): + """A missing, malformed, or untrusted child terminal-status record.""" + + def __init__(self, code: str, message: str) -> None: + self.code = code + super().__init__(message) + + +def _run_gui_generation_inline(awaitable: Awaitable[Any]) -> Any: + """Run the current synchronous Isaac generation stack without claiming Kit's event loop. + + Kit advances its own asyncio loop while GUI simulation steps update the application. The live + AutoData runtime uses async interfaces for orchestration and cancellation, but its Isaac calls + do not suspend. Driving that coroutine inline lets Kit retain main-thread loop ownership. A + future genuinely asynchronous runtime must provide a different integration instead of silently + moving simulator work to another thread. + + Args: + awaitable: Generation operation that must complete without suspending. + """ + + iterator = awaitable.__await__() + try: + next(iterator) + except StopIteration as completion: + return completion.value + except BaseException: + with suppress(BaseException): + iterator.close() + raise + + with suppress(BaseException): + iterator.close() + raise RuntimeError( + "GUI generation unexpectedly suspended; Kit owns the main-thread event loop and the " + "current Isaac generation runtime must complete inline" + ) + + +def build_argument_parser() -> argparse.ArgumentParser: + """Build the complete lightweight CLI parser without importing Isaac or planner packages.""" + + parser = argparse.ArgumentParser( + description=__doc__, + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=( + "Exit codes:\n" + " 0 requested successful episode target reached\n" + " 2 command-line usage error\n" + " 3 request validation or Arena compilation failed\n" + " 4 cuRobo/ScheduleStream runtime preflight failed\n" + " 5 an output target already exists or is unsafe\n" + " 6 Isaac AppLauncher failed\n" + " 7 Arena runtime, planner, or executor setup failed\n" + " 8 generation failed, exhausted its bound, or stopped unrecoverably\n" + " 9 one or more owned resources failed to close\n" + " 10 internal execution or process-handoff error\n" + " 130 interrupted by the operator" + ), + ) + parser.add_argument("request", type=Path, help="Path to an AutoData v1 task request YAML.") + parser.add_argument( + "--format", + choices=("human", "json"), + default="human", + dest="output_format", + help="Summary and error format (default: human).", + ) + parser.add_argument( + "--json", + action="store_const", + const="json", + dest="output_format", + help="Shorthand for --format json.", + ) + parser.add_argument( + "--device", + type=_device_argument, + default="cuda:0", + help="Isaac simulation device: cpu, cuda, or cuda:N (default: cuda:0).", + ) + display_mode = parser.add_mutually_exclusive_group() + display_mode.add_argument( + "--gui", + action="store_false", + dest="headless", + help="Launch the Kit GUI instead of running headless.", + ) + display_mode.add_argument( + "--headless", + action="store_true", + dest="headless", + help="Run without the Kit GUI (default).", + ) + display_mode.add_argument( + "--no-headless", + action="store_false", + dest="headless", + help=argparse.SUPPRESS, + ) + parser.set_defaults(headless=True) + parser.add_argument( + "--enable-cameras", + action="store_true", + help="Enable Arena camera sensors in headless or GUI execution.", + ) + parser.add_argument( + "--runtime-child", + action="store_true", + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--expected-compiled-task-digest", + type=_sha256_argument, + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--expected-motion-backend", + choices=("curobo_v1", "curobo_v2"), + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--expected-schedulestream-application", + choices=("custream", "custream2"), + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--expected-runtime-support-digest", + type=_sha256_argument, + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--status-fd", + type=_status_fd_argument, + help=argparse.SUPPRESS, + ) + return parser + + +def run_cli( + argv: Sequence[str] | None = None, + *, + compiler: Callable[[Path], Any] | None = None, + capability_detector: Callable[[], Any] | None = None, + backend_selector: Callable[[str, Any], Any] | None = None, + child_process_runner: ( + Callable[ + [Sequence[str]], + int | _RuntimeChildProcessResult, + ] + | None + ) = None, + app_launcher_factory_loader: Callable[[], Callable[[Mapping[str, Any]], Any]] | None = None, + runtime_stack_loader: Callable[[], RuntimeStack] | None = None, + terminal_status_writer: Callable[[int | None, int], None] | None = None, + stdout: TextIO | None = None, + stderr: TextIO | None = None, +) -> int: + """Validate in the parent, then execute in an App-first fresh child process.""" + + parser = build_argument_parser() + args = parser.parse_args(argv) + stdout = stdout or sys.stdout + stderr = stderr or sys.stderr + + if args.runtime_child: + missing = [ + option + for option, value in ( + ("--expected-compiled-task-digest", args.expected_compiled_task_digest), + ("--expected-motion-backend", args.expected_motion_backend), + ( + "--expected-schedulestream-application", + args.expected_schedulestream_application, + ), + ( + "--expected-runtime-support-digest", + args.expected_runtime_support_digest, + ), + ) + if value is None + ] + if args.status_fd is None and terminal_status_writer is None: + missing.append("--status-fd") + if missing: + parser.error(f"--runtime-child requires {', '.join(missing)}") + return _run_runtime_child( + args, + compiler=compiler, + capability_detector=capability_detector, + backend_selector=backend_selector, + app_launcher_factory_loader=app_launcher_factory_loader, + runtime_stack_loader=runtime_stack_loader, + terminal_status_writer=terminal_status_writer, + stdout=stdout, + stderr=stderr, + ) + + unexpected_internal_options = [ + option + for option, value in ( + ("--expected-compiled-task-digest", args.expected_compiled_task_digest), + ("--expected-motion-backend", args.expected_motion_backend), + ( + "--expected-schedulestream-application", + args.expected_schedulestream_application, + ), + ( + "--expected-runtime-support-digest", + args.expected_runtime_support_digest, + ), + ("--status-fd", args.status_fd), + ) + if value is not None + ] + if unexpected_internal_options: + parser.error(f"internal options require --runtime-child: {', '.join(unexpected_internal_options)}") + + return _run_parent( + args, + compiler=compiler, + capability_detector=capability_detector, + backend_selector=backend_selector, + child_process_runner=child_process_runner, + stdout=stdout, + stderr=stderr, + ) + + +def _run_parent( + args: argparse.Namespace, + *, + compiler: Callable[[Path], Any] | None, + capability_detector: Callable[[], Any] | None, + backend_selector: Callable[[str, Any], Any] | None, + child_process_runner: ( + Callable[ + [Sequence[str]], + int | _RuntimeChildProcessResult, + ] + | None + ), + stdout: TextIO, + stderr: TextIO, +) -> int: + """Compile and preflight without Isaac, then launch the isolated runtime child.""" + + del stdout + + from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError + + if compiler is None: + from isaac_autodata_interfaces.autonomous.task_compiler import compile_task_request + + compiler = compile_task_request + try: + resolved = compiler(args.request) + except AutonomousValidationError as exc: + failure = CliFailure( + category="request_compilation", + code="request_invalid", + exit_code=ExitCode.REQUEST_COMPILATION_FAILED, + message="The task request could not be compiled.", + details={"issues": [issue.to_dict() for issue in exc.issues]}, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + except Exception as exc: + failure = _unexpected_failure("request_compilation", exc) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + + requested_motion_backend = "unknown" + capabilities: Any | None = None + try: + if capability_detector is None or backend_selector is None: + from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + detect_curobo_runtime, + select_schedulestream_backend, + ) + + capability_detector = capability_detector or detect_curobo_runtime + backend_selector = backend_selector or select_schedulestream_backend + requested_motion_backend = resolved.planner.motion_backend.value + capabilities = capability_detector() + compatibility = backend_selector(requested_motion_backend, capabilities) + preflight = _preflight_to_dict(requested_motion_backend, compatibility) + except Exception as exc: + failure = CliFailure( + category="runtime_preflight", + code="backend_incompatible", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message=_safe_exception_message(exc), + details={ + "capabilities": _capabilities_to_dict(capabilities), + "requested_motion_backend": requested_motion_backend, + "remediation": ( + "Use a reviewed runtime image containing the matching cuRobo and " + "ScheduleStream application. The host environment will not be modified." + ), + }, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + + try: + _attach_runtime_profile(resolved, preflight) + except AutonomousValidationError as exc: + failure = _runtime_capability_failure(exc) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + except Exception as exc: + failure = _unexpected_failure("runtime_capability", exc) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + + try: + _require_fresh_output_targets(resolved) + except Exception as exc: + failure = CliFailure( + category="output_preflight", + code="output_conflict", + exit_code=ExitCode.OUTPUT_CONFLICT, + message=_safe_exception_message(exc), + details={ + "dataset": str(resolved.output.dataset), + "run_log": None if resolved.output.run_log is None else str(resolved.output.run_log), + }, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + + try: + command = _runtime_child_command(args, resolved.digest, preflight) + except Exception as exc: + failure = _unexpected_failure("runtime_handoff", exc) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + process_runner = child_process_runner or _run_runtime_child_process + try: + child_result = process_runner(command) + except KeyboardInterrupt: + failure = CliFailure( + category="interrupted", + code="operator_interrupt", + exit_code=ExitCode.INTERRUPTED, + message="Dataset generation was interrupted by the operator.", + details={"phase": "runtime_child"}, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + except Exception as exc: + failure = CliFailure( + category="runtime_handoff", + code="child_launch_failed", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"The isolated runtime child could not be launched: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + + if isinstance(child_result, _RuntimeChildProcessResult): + try: + child_exit_code = _decode_runtime_status(child_result.status_payload) + except _RuntimeChildStatusError as exc: + failure = CliFailure( + category="runtime_handoff", + code=exc.code, + exit_code=ExitCode.INTERNAL_ERROR, + message=_safe_exception_message(exc), + details={ + "process_return_code": child_result.process_return_code, + "status_bytes": len(child_result.status_payload), + }, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + if child_exit_code == int(ExitCode.SUCCESS) and child_result.process_return_code != 0: + failure = CliFailure( + category="runtime_handoff", + code="child_success_process_failed", + exit_code=ExitCode.INTERNAL_ERROR, + message="The runtime child reported success but then exited abnormally.", + details={"process_return_code": child_result.process_return_code}, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + else: + # Integer results are retained only as a trusted dependency-injection seam for host tests. + child_exit_code = child_result + + known_exit_codes = {int(code) for code in ExitCode} + if isinstance(child_exit_code, bool) or not isinstance(child_exit_code, int): + child_exit_code = -1 + if child_exit_code not in known_exit_codes: + failure = CliFailure( + category="runtime_handoff", + code="child_exit_invalid", + exit_code=ExitCode.INTERNAL_ERROR, + message="The isolated runtime child exited without a recognized AutoData status.", + details={"child_exit_code": child_exit_code}, + ) + _emit_failure(stderr, args.output_format, failure) + return int(failure.exit_code) + return child_exit_code + + +def _run_runtime_child( # noqa: C901 - explicit phase/cleanup boundary is intentionally linear + args: argparse.Namespace, + *, + compiler: Callable[[Path], Any] | None, + capability_detector: Callable[[], Any] | None, + backend_selector: Callable[[str, Any], Any] | None, + app_launcher_factory_loader: Callable[[], Callable[[Mapping[str, Any]], Any]] | None, + runtime_stack_loader: Callable[[], RuntimeStack] | None, + terminal_status_writer: Callable[[int | None, int], None] | None, + stdout: TextIO, + stderr: TextIO, +) -> int: + """Launch Isaac first, recompile and attest the request, then run bounded generation.""" + + cleanup = _CleanupStack() + summary: Any | None = None + run_log_writer: Any | None = None + output_transaction: Any | None = None + request_anchor: Any | None = None + published_artifacts: tuple[Any, ...] = () + resolved: Any | None = None + preflight: dict[str, Any] | None = None + primary_failure: CliFailure | None = None + simulation_app: Any | None = None + phase = "app_launch" + + try: + if args.status_fd is not None: + phase = "runtime_handoff" + os.set_inheritable(args.status_fd, False) + phase = "app_launch" + launcher_loader = app_launcher_factory_loader or _load_app_launcher_factory + app_launcher_factory = launcher_loader() + launcher = app_launcher_factory(_app_launcher_options(args)) + simulation_app = launcher.app + + phase = "request_compilation" + from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError + + if compiler is None: + from isaac_autodata_core.autonomous.output_transaction import RequestDirectoryAnchor + from isaac_autodata_interfaces.autonomous.task_compiler import compile_task_request + + request_anchor = RequestDirectoryAnchor.open(args.request) + compiler = compile_task_request + try: + resolved = compiler(args.request) + except AutonomousValidationError as exc: + raise _CliAbort( + CliFailure( + category="request_compilation", + code="request_invalid_in_runtime_child", + exit_code=ExitCode.REQUEST_COMPILATION_FAILED, + message="The task request could not be recompiled in the runtime child.", + details={"issues": [issue.to_dict() for issue in exc.issues]}, + ) + ) from None + except Exception as exc: + raise _CliAbort(_unexpected_failure("request_compilation", exc)) from None + if request_anchor is not None: + request_anchor.verify_current() + if resolved.digest != args.expected_compiled_task_digest: + raise _CliAbort( + CliFailure( + category="runtime_handoff", + code="compiled_task_digest_mismatch", + exit_code=ExitCode.RUNTIME_SETUP_FAILED, + message="The runtime child compiled a different task and refused to execute it.", + details={ + "actual_compiled_task_digest": resolved.digest, + "expected_compiled_task_digest": args.expected_compiled_task_digest, + }, + ) + ) + + phase = "runtime_preflight" + requested_motion_backend = resolved.planner.motion_backend.value + capabilities: Any | None = None + try: + if capability_detector is None or backend_selector is None: + from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + detect_curobo_runtime, + select_schedulestream_backend, + ) + + capability_detector = capability_detector or detect_curobo_runtime + backend_selector = backend_selector or select_schedulestream_backend + capabilities = capability_detector() + compatibility = backend_selector(requested_motion_backend, capabilities) + preflight = _preflight_to_dict(requested_motion_backend, compatibility) + except Exception as exc: + raise _CliAbort( + _runtime_preflight_failure( + exc, + requested_motion_backend=requested_motion_backend, + capabilities=capabilities, + ) + ) from None + if ( + preflight["selected_motion_backend"] != args.expected_motion_backend + or preflight["schedulestream_application"] != args.expected_schedulestream_application + ): + raise _CliAbort( + CliFailure( + category="runtime_handoff", + code="runtime_selection_mismatch", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message="The runtime child selected a different planner runtime and refused to execute it.", + details={ + "actual_motion_backend": preflight["selected_motion_backend"], + "actual_schedulestream_application": preflight["schedulestream_application"], + "expected_motion_backend": args.expected_motion_backend, + "expected_schedulestream_application": args.expected_schedulestream_application, + }, + ) + ) + + try: + _attach_runtime_profile(resolved, preflight) + except AutonomousValidationError as exc: + raise _CliAbort(_runtime_capability_failure(exc)) from None + if preflight["runtime_profile_digest"] != args.expected_runtime_support_digest: + raise _CliAbort( + CliFailure( + category="runtime_handoff", + code="runtime_support_mismatch", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message="The runtime child admitted a different runtime-support profile.", + details={ + "actual_runtime_support_digest": preflight["runtime_profile_digest"], + "expected_runtime_support_digest": args.expected_runtime_support_digest, + }, + ) + ) + + phase = "output_preflight" + try: + _require_fresh_output_targets(resolved) + except Exception as exc: + raise _CliAbort(_output_conflict_failure(resolved, exc)) from None + + phase = "runtime_setup" + stack = (runtime_stack_loader or _load_runtime_stack)() + transaction_arguments = { + "dataset_path": resolved.output.dataset, + "run_log_path": resolved.output.run_log, + "keep_failed": resolved.output.keep_failed, + } + if request_anchor is None: + transaction_arguments["request_directory"] = Path(args.request).expanduser().resolve(strict=False).parent + else: + transaction_arguments["request_anchor"] = request_anchor + output_transaction = stack.output_transaction_factory(**transaction_arguments) + if resolved.output.run_log is not None: + run_log_writer = output_transaction.open_run_log_writer(stack.run_log_writer_factory) + run_log_writer.append( + _run_started_record( + resolved, + preflight, + ) + ) + runtime_args = _arena_runtime_args(args, resolved) + bundle = stack.arena_runtime_builder( + resolved, + runtime_args, + recording_targets=output_transaction.recording_targets, + allow_output_overwrite=False, + ) + cleanup.push("arena_runtime", bundle.close) + + goal = stack.goal_projector(resolved) + attachment_state = stack.attachment_state_factory() + runtime = stack.runtime_factory( + bundle.env, + bundle.embodiment_adapter, + graph_nodes=tuple(resolved.linked_graph.get("nodes", ())), + attachment_state=attachment_state, + ) + success_verifier = stack.success_verifier_factory(bundle.success_term) + executor = stack.executor_factory( + bundle.env, + bundle.embodiment_adapter, + success_verifier, + attachment_state=attachment_state, + ) + planner = stack.planner_factory( + bundle, + resolved, + compatibility, + attachment_state=attachment_state, + ) + cleanup.push("episode_planner", planner.close) + + generator = stack.generator_factory( + runtime, + planner, + executor, + run_log_writer=run_log_writer, + ) + generation_request = stack.generation_request_factory( + request_digest=resolved.request_digest, + goal=goal, + successful_episodes=resolved.generation.successful_episodes, + max_attempts=resolved.generation.max_attempts, + base_seed=resolved.generation.seed, + num_envs=resolved.generation.num_envs, + keep_failed=resolved.output.keep_failed, + expected_plan_backend=f"schedulestream_{preflight['schedulestream_application']}", + ) + + phase = "generation" + generation = stack.run_loop(generator, generation_request, close=False) + summary = asyncio.run(generation) if args.headless else _run_gui_generation_inline(generation) + summary_dict = _summary_to_dict(summary) + if run_log_writer is not None: + run_log_writer.append({ + "record_type": "run_summary", + "compiled_task_digest": resolved.digest, + **summary_dict, + }) + except _CliAbort as exc: + primary_failure = exc.failure + except KeyboardInterrupt: + primary_failure = CliFailure( + category="interrupted", + code="operator_interrupt", + exit_code=ExitCode.INTERRUPTED, + message="Dataset generation was interrupted by the operator.", + details={"phase": phase}, + ) + except Exception as exc: + primary_failure = _phase_failure(phase, exc) + finally: + cleanup_issues = cleanup.close() + + if cleanup_issues: + if primary_failure is None: + primary_failure = CliFailure( + category="cleanup", + code="resource_cleanup_failed", + exit_code=ExitCode.CLEANUP_FAILED, + message="One or more owned runtime resources failed to close.", + details={ + "cleanup_failures": [issue.to_dict() for issue in cleanup_issues], + "generation_summary": None if summary is None else _summary_to_dict(summary), + }, + ) + else: + primary_failure = primary_failure.with_cleanup(cleanup_issues) + + if primary_failure is None: + assert output_transaction is not None + assert summary is not None + assert resolved is not None + phase = "output_publication" + try: + summary_dict = _summary_to_dict(summary) + commit_record = ( + None + if run_log_writer is None + else { + "generation": summary_dict, + "record_type": "run_committed", + "request_digest": resolved.request_digest, + "compiled_task_digest": resolved.digest, + } + ) + published_artifacts = tuple( + output_transaction.publish( + run_log_writer=run_log_writer, + commit_record=commit_record, + require_failed_dataset=bool(resolved.output.keep_failed and summary_dict["failures"]), + expected_successful_episodes=summary_dict["successes"], + expected_failed_episodes=(summary_dict["failures"] if resolved.output.keep_failed else None), + ) + ) + except Exception as exc: + primary_failure = _output_publication_failure(resolved, exc) + + # Any append/fsync ambiguity can mean the JSONL tail is partial or already durable. Never append + # to that run log again in-process: a second write could concatenate onto a malformed line or + # create contradictory terminal records. Recovery must inspect the existing tail. + run_log_state_uncertain = primary_failure is not None and primary_failure.code in { + "dataset_commit_uncertain", + "run_log_write_uncertain", + } + if primary_failure is not None and run_log_writer is not None and not run_log_state_uncertain: + try: + run_log_writer.append(_terminal_failure_record(resolved, primary_failure)) + except Exception as exc: + if type(exc).__name__ == "RunLogWriteUncertainError": + primary_failure = _run_log_write_uncertain_failure( + "terminal_run_log", + exc, + preceding_failure=primary_failure, + ) + else: + primary_failure = primary_failure.with_cleanup(( + CleanupIssue( + resource="terminal_run_log", + exception_type=type(exc).__name__, + message=_safe_exception_message(exc), + ), + )) + + output_cleanup_issues = _close_output_resources(run_log_writer, output_transaction, request_anchor) + if output_cleanup_issues: + if primary_failure is None: + primary_failure = CliFailure( + category="cleanup", + code="output_cleanup_failed", + exit_code=ExitCode.CLEANUP_FAILED, + message="One or more owned output resources failed to close.", + details={ + "cleanup_failures": [issue.to_dict() for issue in output_cleanup_issues], + "generation_summary": None if summary is None else _summary_to_dict(summary), + }, + ) + else: + primary_failure = primary_failure.with_cleanup(output_cleanup_issues) + + return _finish_runtime_child( + args, + simulation_app=simulation_app, + summary=summary, + resolved=resolved, + preflight=preflight, + published_artifacts=published_artifacts, + primary_failure=primary_failure, + terminal_status_writer=terminal_status_writer, + stdout=stdout, + stderr=stderr, + ) + + +def _finish_runtime_child( + args: argparse.Namespace, + *, + simulation_app: Any | None, + summary: Any | None, + resolved: Any | None, + preflight: Mapping[str, Any] | None, + published_artifacts: tuple[Any, ...], + primary_failure: CliFailure | None, + terminal_status_writer: Callable[[int | None, int], None] | None, + stdout: TextIO, + stderr: TextIO, +) -> int: + """Publish one terminal result before making SimulationApp shutdown the final operation.""" + + if primary_failure is not None: + terminal_exit_code = int(primary_failure.exit_code) + try: + _emit_failure(stderr, args.output_format, primary_failure) + except Exception as exc: + primary_failure = _unexpected_failure("terminal_output", exc) + terminal_exit_code = int(primary_failure.exit_code) + _emit_failure_best_effort(stderr, args.output_format, primary_failure) + else: + assert summary is not None + assert resolved is not None + assert preflight is not None + try: + result = _build_result(resolved, preflight, summary, published_artifacts=published_artifacts) + if args.output_format == "json": + _write_json(stdout, result) + else: + _write_human_summary(stdout, result) + terminal_exit_code = int(ExitCode.SUCCESS if summary.target_reached else ExitCode.GENERATION_INCOMPLETE) + except Exception as exc: + primary_failure = _unexpected_failure("terminal_output", exc) + terminal_exit_code = int(primary_failure.exit_code) + _emit_failure(stderr, args.output_format, primary_failure) + + try: + _flush_terminal_streams(stdout, stderr) + except Exception as exc: + primary_failure = CliFailure( + category="terminal_output", + code="terminal_flush_failed", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"Terminal output could not be flushed: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + terminal_exit_code = int(primary_failure.exit_code) + _emit_failure_best_effort(stderr, args.output_format, primary_failure) + _flush_best_effort(stdout, stderr) + + status_writer = terminal_status_writer or _write_runtime_status + try: + status_writer(args.status_fd, terminal_exit_code) + except Exception as exc: + status_failure = CliFailure( + category="runtime_handoff", + code="child_status_write_failed", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"The runtime child could not publish terminal status: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + terminal_exit_code = int(status_failure.exit_code) + _emit_failure_best_effort(stderr, args.output_format, status_failure) + _flush_best_effort(stdout, stderr) + + # SimulationApp.close() can terminate the process and never return. It must remain the final + # operation, after ordinary resources, terminal output, and the out-of-band status record. + if simulation_app is not None: + simulation_app.close() + return terminal_exit_code + + +def main(argv: Sequence[str] | None = None) -> int: + """Run the production dataset generation entrypoint.""" + + return run_cli(argv) + + +def _runtime_child_command( + args: argparse.Namespace, + compiled_task_digest: str, + preflight: Mapping[str, Any], +) -> tuple[str, ...]: + """Build the complete allowlisted argv for the isolated, attested runtime child.""" + + expected_digest = _sha256_argument(compiled_task_digest) + motion_backend = preflight.get("selected_motion_backend") + application = preflight.get("schedulestream_application") + runtime_support_digest = preflight.get("runtime_profile_digest") + if motion_backend not in ("curobo_v1", "curobo_v2"): + raise ValueError("preflight returned no supported selected motion backend") + if application not in ("custream", "custream2"): + raise ValueError("preflight returned no supported ScheduleStream application") + runtime_support_digest = _sha256_argument(runtime_support_digest) + command = [ + sys.executable, + "-u", + str(Path(__file__).resolve()), + str(args.request.expanduser().resolve(strict=False)), + "--runtime-child", + "--expected-compiled-task-digest", + expected_digest, + "--expected-motion-backend", + motion_backend, + "--expected-schedulestream-application", + application, + "--expected-runtime-support-digest", + runtime_support_digest, + "--format", + args.output_format, + "--device", + args.device, + "--headless" if args.headless else "--no-headless", + ] + if args.enable_cameras: + command.append("--enable-cameras") + return tuple(command) + + +def _run_runtime_child_process( + command: Sequence[str], + *, + wall_timeout_s: float = _RUNTIME_CHILD_WALL_TIMEOUT_S, + terminate_grace_s: float = _RUNTIME_CHILD_TERMINATE_GRACE_S, +) -> _RuntimeChildProcessResult: + """Supervise the child process group and its private bounded terminal-status pipe.""" + + import os + import signal + import subprocess + import time + + if "--status-fd" in command: + raise ValueError("runtime child command must not provide its own status descriptor") + if not command or any(not isinstance(argument, str) or "\x00" in argument for argument in command): + raise ValueError("runtime child command contains an invalid argument") + _validate_runtime_supervision_duration("wall_timeout_s", wall_timeout_s) + _validate_runtime_supervision_duration("terminate_grace_s", terminate_grace_s) + + read_fd, write_fd = os.pipe() + process: Any | None = None + previous_signal_handlers: dict[int, Any] = {} + forwarded_signals: list[int] = [] + cleanup_attempted = False + try: + child_command = (*command, "--status-fd", str(write_fd)) + try: + process = subprocess.Popen( + child_command, + close_fds=True, + pass_fds=(write_fd,), + start_new_session=True, + ) + finally: + os.close(write_fd) + write_fd = -1 + + previous_signal_handlers = _install_runtime_signal_forwarders(process, forwarded_signals) + deadline_s = time.monotonic() + wall_timeout_s + try: + timed_out = _wait_for_runtime_child( + process, + deadline_s=deadline_s, + forwarded_signals=forwarded_signals, + ) + except KeyboardInterrupt: + cleanup_attempted = True + _terminate_runtime_process_group( + process, + initial_signal=signal.SIGINT, + terminate_grace_s=terminate_grace_s, + ) + raise + + shutdown_interrupted = False + if timed_out: + cleanup_attempted = True + shutdown_interrupted = _terminate_runtime_process_group( + process, + initial_signal=signal.SIGTERM, + terminate_grace_s=terminate_grace_s, + ) + elif forwarded_signals: + # The temporary signal handler already forwarded the original signal verbatim. + cleanup_attempted = True + shutdown_interrupted = _terminate_runtime_process_group( + process, + initial_signal=None, + terminate_grace_s=terminate_grace_s, + ) + elif _runtime_process_group_exists(process.pid): + # The direct child is the sole owner of this private session. A surviving group after + # it exits is an unexpected descendant leak, even if the child published success. + cleanup_attempted = True + _terminate_runtime_process_group( + process, + initial_signal=signal.SIGTERM, + terminate_grace_s=terminate_grace_s, + ) + raise RuntimeError("runtime child exited while descendants remained in its process group") + + assert process.returncode is not None, "runtime child must be reaped before status collection" + status_payload = _read_runtime_status_pipe(read_fd) + result = _RuntimeChildProcessResult( + process_return_code=process.returncode, + status_payload=status_payload, + ) + _restore_runtime_signal_handlers(previous_signal_handlers) + previous_signal_handlers = {} + if forwarded_signals or shutdown_interrupted: + raise KeyboardInterrupt + return result + except BaseException: + if ( + process is not None + and not cleanup_attempted + and (process.poll() is None or _runtime_process_group_exists(process.pid)) + ): + cleanup_attempted = True + _terminate_runtime_process_group( + process, + initial_signal=signal.SIGTERM, + terminate_grace_s=terminate_grace_s, + ) + raise + finally: + try: + _restore_runtime_signal_handlers(previous_signal_handlers) + finally: + try: + if write_fd >= 0: + os.close(write_fd) + finally: + os.close(read_fd) + + +def _validate_runtime_supervision_duration(name: str, value: float) -> None: + """Reject supervision bounds that could disable or destabilize the parent guardrail.""" + + import math + + if isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be a finite positive number of seconds") + + +def _install_runtime_signal_forwarders(process: Any, forwarded_signals: list[int]) -> dict[int, Any]: + """Temporarily forward operator termination signals to the isolated child group.""" + + import signal + + previous_handlers: dict[int, Any] = {} + + def forward_signal(signum: int, _frame: Any) -> None: + forwarded_signals.append(signum) + _signal_runtime_process_group(process, signum) + + try: + for signum in (signal.SIGINT, signal.SIGTERM): + previous_handler = signal.getsignal(signum) + signal.signal(signum, forward_signal) + previous_handlers[signum] = previous_handler + except ValueError: + # Python only permits signal-handler installation in the main thread. Direct callers in a + # worker thread still get KeyboardInterrupt cleanup, but the production CLI runs here. + _restore_runtime_signal_handlers(previous_handlers) + return {} + except BaseException: + _restore_runtime_signal_handlers(previous_handlers) + raise + return previous_handlers + + +def _restore_runtime_signal_handlers(previous_handlers: Mapping[int, Any]) -> None: + """Restore every signal disposition replaced by the runtime supervisor.""" + + import signal + + for signum, previous_handler in previous_handlers.items(): + signal.signal(signum, previous_handler) + + +def _wait_for_runtime_child( + process: Any, + *, + deadline_s: float, + forwarded_signals: Sequence[int], +) -> bool: + """Wait until exit, operator interruption, or the absolute runtime deadline.""" + + import subprocess + import time + + while process.poll() is None and not forwarded_signals: + remaining_s = deadline_s - time.monotonic() + if remaining_s <= 0: + return True + try: + process.wait(timeout=min(_RUNTIME_CHILD_WAIT_POLL_S, remaining_s)) + except subprocess.TimeoutExpired: + continue + return False + + +def _terminate_runtime_process_group( + process: Any, + *, + initial_signal: int | None, + terminate_grace_s: float, +) -> bool: + """Gracefully stop, forcibly kill, and reap an isolated runtime process group.""" + + import signal + import subprocess + import time + + def target_exists() -> bool: + return process.poll() is None or _runtime_process_group_exists(process.pid) + + interrupted = False + if target_exists() and initial_signal is not None: + _signal_runtime_process_group(process, initial_signal) + + grace_deadline_s = time.monotonic() + terminate_grace_s + while target_exists(): + remaining_s = grace_deadline_s - time.monotonic() + if remaining_s <= 0: + break + poll_s = min(_RUNTIME_CHILD_WAIT_POLL_S, remaining_s) + try: + if process.poll() is None: + process.wait(timeout=poll_s) + else: + time.sleep(poll_s) + except subprocess.TimeoutExpired: + continue + except KeyboardInterrupt: + interrupted = True + _signal_runtime_process_group(process, signal.SIGINT) + + if target_exists(): + _signal_runtime_process_group(process, signal.SIGKILL) + + # SIGKILL cannot be handled. Verify the entire private group disappears within another bounded + # grace window; reaping only the direct child is not evidence that descendants are gone. + kill_deadline_s = time.monotonic() + terminate_grace_s + while target_exists(): + remaining_s = kill_deadline_s - time.monotonic() + poll_s = max(0.0, min(_RUNTIME_CHILD_WAIT_POLL_S, remaining_s)) + try: + if process.poll() is None: + process.wait(timeout=poll_s) + elif remaining_s <= 0: + raise RuntimeError("runtime process group remained alive after SIGKILL") + time.sleep(poll_s) + except subprocess.TimeoutExpired: + if remaining_s <= 0: + raise RuntimeError("runtime process group remained alive after SIGKILL") from None + continue + except KeyboardInterrupt: + interrupted = True + _signal_runtime_process_group(process, signal.SIGKILL) + return interrupted + + +def _runtime_process_group_exists(process_group_id: int) -> bool: + """Return whether the isolated runtime process group still has any member.""" + + import os + + try: + os.killpg(process_group_id, 0) + except ProcessLookupError: + return False + except PermissionError: + # The group exists even if a changed credential prevents signaling it. Treat this as live + # so supervision fails closed instead of claiming cleanup. + return True + return True + + +def _signal_runtime_process_group(process: Any, signum: int) -> None: + """Signal the new-session process group, tolerating an exit race.""" + + import os + + with suppress(ProcessLookupError): + os.killpg(process.pid, signum) + + +def _read_runtime_status_pipe(read_fd: int) -> bytes: + """Read only currently available bounded bytes after the direct child has terminated.""" + + import os + + os.set_blocking(read_fd, False) + payload = bytearray() + while len(payload) <= _MAX_RUNTIME_STATUS_BYTES: + try: + chunk = os.read(read_fd, _MAX_RUNTIME_STATUS_BYTES + 1 - len(payload)) + except BlockingIOError: + break + if not chunk: + break + payload.extend(chunk) + return bytes(payload) + + +def _write_runtime_status(status_fd: int | None, exit_code: int) -> None: + """Write and close one validated status record before SimulationApp termination.""" + + if status_fd is None: + return + + import os + + try: + payload = _encode_runtime_status(exit_code) + remaining = memoryview(payload) + while remaining: + written = os.write(status_fd, remaining) + if written <= 0: + raise OSError("status pipe write made no progress") + remaining = remaining[written:] + finally: + os.close(status_fd) + + +def _encode_runtime_status(exit_code: int) -> bytes: + if isinstance(exit_code, bool) or not isinstance(exit_code, int): + raise ValueError("runtime child exit code must be an integer") + if exit_code not in {int(code) for code in ExitCode}: + raise ValueError("runtime child exit code is not recognized") + payload = ( + json.dumps( + { + "exit_code": exit_code, + "protocol_version": _RUNTIME_STATUS_PROTOCOL_VERSION, + }, + sort_keys=True, + separators=(",", ":"), + allow_nan=False, + ).encode("utf-8") + + b"\n" + ) + assert len(payload) <= _MAX_RUNTIME_STATUS_BYTES + return payload + + +def _decode_runtime_status(payload: bytes) -> int: + if not isinstance(payload, bytes): + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status record was not a byte sequence.", + ) + if not payload: + raise _RuntimeChildStatusError( + "child_status_missing", + "The runtime child exited without publishing terminal status; its process exit code is not trusted.", + ) + if len(payload) > _MAX_RUNTIME_STATUS_BYTES: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status record exceeded its byte limit.", + ) + if not payload.endswith(b"\n") or b"\n" in payload[:-1]: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status channel did not contain exactly one record.", + ) + try: + value = json.loads( + payload.decode("utf-8"), + object_pairs_hook=_runtime_status_object, + ) + except (UnicodeDecodeError, json.JSONDecodeError, ValueError) as exc: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status record was not valid UTF-8 JSON.", + ) from exc + if not isinstance(value, dict) or set(value) != {"exit_code", "protocol_version"}: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status record had an invalid schema.", + ) + if type(value["protocol_version"]) is not int or value["protocol_version"] != _RUNTIME_STATUS_PROTOCOL_VERSION: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status protocol version was not supported.", + ) + exit_code = value["exit_code"] + if type(exit_code) is not int or exit_code not in {int(code) for code in ExitCode}: + raise _RuntimeChildStatusError( + "child_status_malformed", + "The runtime child terminal-status exit code was not recognized.", + ) + return exit_code + + +def _runtime_status_object(pairs: list[tuple[str, Any]]) -> dict[str, Any]: + value: dict[str, Any] = {} + for key, item in pairs: + if key in value: + raise ValueError("duplicate terminal-status field") + value[key] = item + return value + + +def _load_app_launcher_factory() -> Callable[[Mapping[str, Any]], Any]: + """Import AppLauncher first in the fresh child, before Arena or request compilation.""" + + from isaaclab.app import AppLauncher + + return AppLauncher + + +def _load_runtime_stack() -> RuntimeStack: + """Import runtime and provider implementations only after SimulationApp starts.""" + + from isaac_autodata_core.autonomous.attempt_generation import AttemptGenerator + from isaac_autodata_core.autonomous.dataset_generation import DatasetGenerationRequest, generate_dataset + from isaac_autodata_core.autonomous.output_transaction import OutputTransaction + from isaac_autodata_core.autonomous.run_log import RunLogWriter + from isaac_autodata_interfaces.autonomous.arena_environment import build_arena_runtime, goal_predicates_from_request + from isaac_autodata_interfaces.autonomous.isaaclab_runtime import ( + AttachmentState, + IsaacLabAttemptRuntime, + IsaacLabPlanExecutor, + success_term_verifier, + ) + from isaac_autodata_interfaces.autonomous.schedulestream.episode_planner import ( + create_schedulestream_episode_planner, + ) + + return RuntimeStack( + output_transaction_factory=OutputTransaction.reserve, + arena_runtime_builder=build_arena_runtime, + goal_projector=goal_predicates_from_request, + attachment_state_factory=AttachmentState, + runtime_factory=IsaacLabAttemptRuntime, + success_verifier_factory=success_term_verifier, + executor_factory=IsaacLabPlanExecutor, + planner_factory=create_schedulestream_episode_planner, + run_log_writer_factory=RunLogWriter, + generator_factory=AttemptGenerator, + generation_request_factory=DatasetGenerationRequest, + run_loop=generate_dataset, + ) + + +def _app_launcher_options(args: argparse.Namespace) -> dict[str, Any]: + options: dict[str, Any] = { + "device": args.device, + "enable_cameras": args.enable_cameras, + "headless": args.headless, + } + if not args.headless: + # Current Isaac Lab resolves an omitted visualizer to headless execution. Selecting the Kit + # visualizer is therefore the explicit GUI intent; ``headless=False`` alone is insufficient. + options["visualizer"] = ["kit"] + return options + + +def _arena_runtime_args(args: argparse.Namespace, resolved: Any) -> argparse.Namespace: + """Return the allowlisted Arena builder arguments for deterministic source-free execution.""" + + return argparse.Namespace( + device=args.device, + disable_fabric=False, + enable_cameras=args.enable_cameras, + env_spacing=30.0, + headless=args.headless, + language_instruction=None, + mimic=False, + num_envs=resolved.generation.num_envs, + placement_seed=resolved.generation.seed, + presets=None, + random_yaw_init=False, + resolve_on_reset=None, + seed=resolved.generation.seed, + solve_relations=True, + ) + + +def _preflight_to_dict(requested_motion_backend: str, compatibility: Any) -> dict[str, Any]: + selected_motion_backend = getattr(compatibility, "motion_backend", None) + application = getattr(compatibility, "schedulestream_application", None) + selected_capabilities = getattr(compatibility, "capabilities", None) + if selected_motion_backend not in ("curobo_v1", "curobo_v2"): + raise ValueError("backend selector returned no supported motion backend") + if application not in ("custream", "custream2"): + raise ValueError("backend selector returned no supported ScheduleStream application") + capabilities_dict = ( + selected_capabilities.to_dict() + if callable(getattr(selected_capabilities, "to_dict", None)) + else {"summary": f"unavailable ({type(selected_capabilities).__name__})"} + ) + if not isinstance(capabilities_dict, dict): + raise ValueError("backend selector capabilities are not JSON compatible") + return { + "capabilities": capabilities_dict, + "requested_motion_backend": requested_motion_backend, + "schedulestream_application": application, + "selected_motion_backend": selected_motion_backend, + "status": "passed", + } + + +def _capabilities_to_dict(capabilities: Any | None) -> dict[str, Any]: + if capabilities is None: + return {"status": "probe_failed_before_result"} + serializer = getattr(capabilities, "to_dict", None) + if not callable(serializer): + return {"status": f"unavailable ({type(capabilities).__name__})"} + try: + value = serializer() + except Exception as exc: + return { + "serialization_error": _safe_exception_message(exc), + "status": "unavailable", + } + return value if isinstance(value, dict) else {"status": "invalid_capability_report"} + + +def _attach_runtime_profile(resolved: Any, preflight: dict[str, Any]) -> None: + """Run the import-free product gate and attach its attestable identity to preflight.""" + + from isaac_autodata_interfaces.autonomous.runtime_support import validate_runtime_support + + profile = validate_runtime_support( + resolved, + motion_backend=preflight["selected_motion_backend"], + schedulestream_application=preflight["schedulestream_application"], + ) + preflight["runtime_profile"] = profile.to_dict() + preflight["runtime_profile_digest"] = profile.digest + + +def _runtime_capability_failure(exc: Any) -> CliFailure: + issues = getattr(exc, "issues", ()) + return CliFailure( + category="runtime_capability", + code="request_capability_unsupported", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message="The compiled task request is outside the currently executable product profile.", + details={ + "issues": [issue.to_dict() for issue in issues], + "remediation": ( + "Use the reviewed single-environment Franka PickAndPlace/on profile with " + "curobo_v1/custream, or add and validate a new live runtime profile." + ), + }, + ) + + +def _runtime_preflight_failure( + exc: Exception, + *, + requested_motion_backend: str, + capabilities: Any | None, +) -> CliFailure: + return CliFailure( + category="runtime_preflight", + code="backend_incompatible", + exit_code=ExitCode.RUNTIME_PREFLIGHT_FAILED, + message=_safe_exception_message(exc), + details={ + "capabilities": _capabilities_to_dict(capabilities), + "requested_motion_backend": requested_motion_backend, + "remediation": ( + "Use a reviewed runtime image containing the matching cuRobo and " + "ScheduleStream application. The host environment will not be modified." + ), + }, + ) + + +def _output_conflict_failure(resolved: Any, exc: Exception) -> CliFailure: + return CliFailure( + category="output_preflight", + code="output_conflict", + exit_code=ExitCode.OUTPUT_CONFLICT, + message=_safe_exception_message(exc), + details={ + "dataset": str(resolved.output.dataset), + "run_log": None if resolved.output.run_log is None else str(resolved.output.run_log), + }, + ) + + +def _output_publication_failure(resolved: Any, exc: Exception) -> CliFailure: + if isinstance(exc, FileExistsError): + return _output_conflict_failure(resolved, exc) + if type(exc).__name__ == "RunLogWriteUncertainError": + return _run_log_write_uncertain_failure("output_publication", exc) + commit_uncertain = type(exc).__name__ == "DatasetCommitUncertainError" + return CliFailure( + category="output_publication", + code="dataset_commit_uncertain" if commit_uncertain else "dataset_publication_failed", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"The staged dataset could not be durably published: {_safe_exception_message(exc)}", + details={ + "dataset": str(resolved.output.dataset), + "exception_type": type(exc).__name__, + "recovery": ( + "Dataset links are durable but run-log commit durability is unknown; inspect the held run log " + "and artifact digests before any retry or removal." + if commit_uncertain + else "No terminal commit was recorded; inspect the run log before retrying." + ), + "run_log": None if resolved.output.run_log is None else str(resolved.output.run_log), + }, + ) + + +def _terminal_failure_record(resolved: Any | None, failure: CliFailure) -> dict[str, Any]: + assert failure.code not in { + "dataset_commit_uncertain", + "run_log_write_uncertain", + }, "a run log with uncertain state must not be written again" + if failure.code == "operator_interrupt": + record_type = "run_interrupted" + elif failure.category == "cleanup": + record_type = "run_cleanup_failed" + else: + record_type = "run_aborted" + return { + "failure": { + "category": failure.category, + "code": failure.code, + "exit_code": int(failure.exit_code), + "message": failure.message, + }, + "record_type": record_type, + "request_digest": None if resolved is None else resolved.request_digest, + "compiled_task_digest": None if resolved is None else resolved.digest, + } + + +def _close_output_resources( + run_log_writer: Any | None, + output_transaction: Any | None, + request_anchor: Any | None, +) -> tuple[CleanupIssue, ...]: + issues: list[CleanupIssue] = [] + for resource, owned in ( + ("run_log_writer", run_log_writer), + ("output_transaction", output_transaction), + ("request_anchor", request_anchor), + ): + if owned is None: + continue + closer = getattr(owned, "close", None) + if not callable(closer): + issues.append( + CleanupIssue( + resource=resource, + exception_type="TypeError", + message=f"{resource} does not expose close()", + ) + ) + continue + try: + closer() + except Exception as exc: + issues.append( + CleanupIssue( + resource=resource, + exception_type=type(exc).__name__, + message=_safe_exception_message(exc), + ) + ) + return tuple(issues) + + +def _require_fresh_output_targets(resolved: Any) -> None: + dataset_path = Path(resolved.output.dataset) + paths = [("dataset", dataset_path)] + if resolved.output.keep_failed: + paths.append(( + "failed dataset", + dataset_path.with_name(f"{dataset_path.stem}_failed{dataset_path.suffix}"), + )) + if resolved.output.run_log is not None: + paths.append(("run_log", Path(resolved.output.run_log))) + for label, path in paths: + if path.exists() or path.is_symlink(): + raise FileExistsError(f"refusing to overwrite existing {label} output: {path}") + + +def _run_started_record(resolved: Any, preflight: Mapping[str, Any]) -> dict[str, Any]: + return { + "preflight": dict(preflight), + "record_type": "run_started", + "request_digest": resolved.request_digest, + "compiled_task": resolved.to_dict(), + "compiled_task_digest": resolved.digest, + } + + +def _summary_to_dict(summary: Any) -> dict[str, Any]: + value = summary.to_dict() + if not isinstance(value, dict): + raise TypeError("generation summary must serialize to a mapping") + if type(getattr(summary, "target_reached", None)) is not bool: + raise TypeError("generation summary target_reached must be a boolean") + return value + + +def _build_result( + resolved: Any, + preflight: Mapping[str, Any], + summary: Any, + *, + published_artifacts: tuple[Any, ...], +) -> dict[str, Any]: + return { + "backend": dict(preflight), + "cleanup": {"completed": True}, + "environment": { + "graph_digest": resolved.graph_digest, + "name": resolved.environment_name, + }, + "generation": _summary_to_dict(summary), + "output": { + "artifacts": [_artifact_to_dict(artifact) for artifact in published_artifacts], + "dataset": str(resolved.output.dataset), + "run_log": None if resolved.output.run_log is None else str(resolved.output.run_log), + }, + "request": { + "digest": resolved.request_digest, + "name": resolved.name, + "compiled_task_digest": resolved.digest, + }, + "status": "completed" if summary.target_reached else "incomplete", + } + + +def _artifact_to_dict(artifact: Any) -> dict[str, Any]: + serializer = getattr(artifact, "to_dict", None) + if not callable(serializer): + raise TypeError("published artifact must expose to_dict()") + value = serializer() + if not isinstance(value, dict): + raise TypeError("published artifact must serialize to a mapping") + return value + + +def _phase_failure(phase: str, exc: Exception) -> CliFailure: + if type(exc).__name__ == "RunLogWriteUncertainError": + return _run_log_write_uncertain_failure(phase, exc) + if phase == "app_launch": + return CliFailure( + category="app_launch", + code="app_launch_failed", + exit_code=ExitCode.APP_LAUNCH_FAILED, + message=f"Isaac AppLauncher failed: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + if phase == "runtime_setup": + return CliFailure( + category="runtime_setup", + code="runtime_setup_failed", + exit_code=ExitCode.RUNTIME_SETUP_FAILED, + message=f"Runtime setup failed: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + return CliFailure( + category="generation", + code="generation_failed", + exit_code=ExitCode.GENERATION_INCOMPLETE, + message=f"Dataset generation failed: {_safe_exception_message(exc)}", + details={"exception_type": type(exc).__name__}, + ) + + +def _run_log_write_uncertain_failure( + phase: str, + exc: Exception, + *, + preceding_failure: CliFailure | None = None, +) -> CliFailure: + """Classify a run_log append whose durable tail state cannot be proven.""" + + details: dict[str, Any] = { + "exception_type": type(exc).__name__, + "phase": phase, + "recovery": ( + "Do not append to or automatically retry this run log. Inspect its existing JSONL tail " + "and durable artifact identities to determine whether the attempted record is absent, partial, or " + "already committed before any manual recovery." + ), + } + if preceding_failure is not None: + details["preceding_failure"] = { + "category": preceding_failure.category, + "code": preceding_failure.code, + "exit_code": int(preceding_failure.exit_code), + } + return CliFailure( + category="run_log", + code="run_log_write_uncertain", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"RunLog append durability is unknown: {_safe_exception_message(exc)}", + details=details, + ) + + +def _unexpected_failure(category: str, exc: Exception) -> CliFailure: + return CliFailure( + category=category, + code="internal_error", + exit_code=ExitCode.INTERNAL_ERROR, + message=f"Unexpected {type(exc).__name__}: {_safe_exception_message(exc)}", + details={ + "remediation": "Report this bounded error to AutoData maintainers.", + }, + ) + + +def _emit_failure(stream: TextIO, output_format: str, failure: CliFailure) -> None: + payload = { + "category": failure.category, + "code": failure.code, + "details": dict(failure.details), + "exit_code": int(failure.exit_code), + "message": failure.message, + } + if output_format == "json": + _write_json(stream, {"error": payload}) + return + stream.write(f"ERROR [{failure.category}/{failure.code}] (exit {int(failure.exit_code)}): {failure.message}\n") + issues = failure.details.get("issues") + if isinstance(issues, list): + for issue in issues: + if isinstance(issue, Mapping): + stream.write( + f" - {issue.get('path', '$')} [{issue.get('code', 'invalid')}]: {issue.get('message', '')}\n" + ) + remediation = failure.details.get("remediation") + if isinstance(remediation, str): + stream.write(f" Remediation: {remediation}\n") + stream.flush() + + +def _emit_failure_best_effort(stream: TextIO, output_format: str, failure: CliFailure) -> None: + try: + _emit_failure(stream, output_format, failure) + except Exception: + return + + +def _flush_terminal_streams(stdout: TextIO, stderr: TextIO) -> None: + issues: list[str] = [] + for name, stream in (("stdout", stdout), ("stderr", stderr)): + try: + stream.flush() + except Exception as exc: + issues.append(f"{name}: {_safe_exception_message(exc)}") + if issues: + raise OSError("; ".join(issues)) + + +def _flush_best_effort(stdout: TextIO, stderr: TextIO) -> None: + for stream in (stdout, stderr): + try: + stream.flush() + except Exception: + continue + + +def _write_human_summary(stream: TextIO, result: Mapping[str, Any]) -> None: + generation = result["generation"] + backend = result["backend"] + request = result["request"] + output = result["output"] + lines = [ + ( + "Dataset generation completed." + if result["status"] == "completed" + else "Dataset generation stopped before reaching its target." + ), + f" Request: {json.dumps(request['name'], ensure_ascii=False)}", + f" Request digest: {request['digest']}", + f" Compiled task digest: {request['compiled_task_digest']}", + f" Motion backend: {backend['selected_motion_backend']}", + f" ScheduleStream application: {backend['schedulestream_application']}", + ( + " Generation: " + f"attempts={generation['attempts']}, successes={generation['successes']}, " + "failures=" + f"{generation['failures']}, requested_successes={generation['requested_successful_episodes']}" + ), + f" Stop reason: {generation['stop_reason']}", + f" Target reached: {str(generation['target_reached']).lower()}", + f" Dataset: {json.dumps(output['dataset'], ensure_ascii=False)}", + f" RunLog: {json.dumps(output['run_log'], ensure_ascii=False)}", + " Cleanup completed: true", + ] + stream.write("\n".join(lines) + "\n") + stream.flush() + + +def _write_json(stream: TextIO, value: Mapping[str, Any]) -> None: + stream.write(json.dumps(value, indent=2, sort_keys=True, ensure_ascii=False, allow_nan=False) + "\n") + stream.flush() + + +def _safe_exception_message(exc: BaseException) -> str: + message = " ".join(str(exc).splitlines()).replace("\x00", "").strip() + return (message or "no additional details")[:2048] + + +def _device_argument(value: str) -> str: + if value in ("cpu", "cuda"): + return value + if value.startswith("cuda:") and value[5:].isdigit(): + return value + raise argparse.ArgumentTypeError("device must be cpu, cuda, or cuda:N") + + +def _sha256_argument(value: str) -> str: + if len(value) == 64 and all(character in "0123456789abcdef" for character in value): + return value + raise argparse.ArgumentTypeError("resolved digest must be a lowercase SHA-256 hex string") + + +def _status_fd_argument(value: str) -> int: + try: + status_fd = int(value, 10) + except ValueError as exc: + raise argparse.ArgumentTypeError("status descriptor must be a decimal integer") from exc + if status_fd < 3 or status_fd > 1_048_576: + raise argparse.ArgumentTypeError("status descriptor is outside the allowed range") + return status_fd + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/isaac_autodata_examples/tasks/franka_cube_stack_schedulestream.yaml b/isaac_autodata_examples/tasks/franka_cube_stack_schedulestream.yaml new file mode 100644 index 0000000..e9bac6c --- /dev/null +++ b/isaac_autodata_examples/tasks/franka_cube_stack_schedulestream.yaml @@ -0,0 +1,48 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +# Task descriptor for the ScheduleStream algorithm on the Franka cube-stack task. +# +# ScheduleStream plans the WHOLE task from scratch (custream2.solve_tamp) rather than transforming +# recorded source-demo subtasks, so a single subtask per EEF is declared: the plug-in solves TAMP +# once and returns the entire trajectory. The symbolic goal (stack the cubes) is derived from the +# env's success term, not from these subtasks. A source dataset is still required by the data-gen +# infrastructure (env name + pool scaffolding) but its demo *content* is not replayed. + +name: franka_cube_stack_schedulestream +description: Stack red, green, and blue cubes into a single tower (ScheduleStream TAMP). +algo: schedulestream + +# Only the fields that differ from GenerationPolicy defaults and matter to ScheduleStream are set. +# The MimicGen source-demo knobs (select_src_*, transform_first_robot_pose, +# interpolate_from_last_target_pose), the None-defaults (source_dataset_path, generation_path, +# task_name, keep_failed, use_navigation_controller) and use_skillgen (overwritten from the +# algorithm class in main()) are all omitted. +generation_policy: + seed: 0 + num_trials: 1 + # guarantee_success: false -> env_loop stops on num_ATTEMPTS >= num_trials (not successes), so a + # failing/erroring attempt does NOT retry forever. The run script also forces this via + # --no-guarantee_success. Flip back to true for real data generation (retry until num_trials + # successful demos are collected). + guarantee_success: false + +subtasks: + franka: + # Single, whole-task subtask. ScheduleStream plans the whole task itself, so the MimicGen + # subtask-transformation fields (object_ref, selection_strategy, action_noise, + # num_interpolation_steps, num_fixed_steps, apply_noise_during_interpolation, + # subtask_term_offset_range) are unused and omitted — their defaults apply (and the terminal + # subtask's default term offset of [0, 0] is what the data-gen validation requires). + - description: Plan and execute the full cube-stacking task with ScheduleStream. + # ScheduleStream solver/debug config (ScheduleStreamSubtaskAlgoParams). ScheduleStream plans + # the whole task in one shot, so these task-wide knobs live on its single subtask instead of + # the shared CLI. Read at runtime via datastream.get_subtask_algo_params(). + algo_params: + collisions: true + max_time: 60.0 + profile: false + hold: null # set to an int (e.g. 100) to skip TAMP and hold the current config + animate: false diff --git a/isaac_autodata_interfaces/autonomous/__init__.py b/isaac_autodata_interfaces/autonomous/__init__.py new file mode 100644 index 0000000..ed118e2 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/__init__.py @@ -0,0 +1,93 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Pure request envelope and lazy Arena bridge for autonomous AutoData generation.""" + +from isaac_autodata_interfaces.autonomous.arena_bridge import ( + REQUIRED_ARENA_CAPABILITY, + REQUIRED_ARENA_COMMIT, + ArenaIntentBridge, + LazyArenaIntentBridge, + build_arena_compilation_result, + extract_goal_stages, +) +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import ( + FRANKA_PICK_CUBE_INTO_BOWL, + AutonomousTaskProfile, + PickPlaceSuccessThresholds, +) +from isaac_autodata_interfaces.autonomous.runtime_support import ( + CURRENT_RUNTIME_SUPPORT, + RUNTIME_SUPPORT_SCHEMA_VERSION, + RuntimeSupportError, + RuntimeSupportProfile, + validate_runtime_support, +) +from isaac_autodata_interfaces.autonomous.task_compiler import ( + TASK_COMPILER_VERSION, + compile_loaded_task_request, + compile_task_request, +) +from isaac_autodata_interfaces.autonomous.task_request import ( + TASK_REQUEST_SCHEMA_VERSION, + load_task_request, + task_request_from_dict, +) +from isaac_autodata_interfaces.autonomous.task_request_types import ( + ArenaCompilationResult, + CompiledTaskRequest, + CompilerTraceEvent, + GenerationConfig, + GoalStage, + MotionBackend, + OutputConfig, + PlannerBackend, + PlannerConfig, + ResolvedOutputConfig, + SpatialGoalConstraint, + TaskRequest, + canonical_json, + sha256_json, +) + +__all__ = [ + "TASK_REQUEST_SCHEMA_VERSION", + "TASK_COMPILER_VERSION", + "RUNTIME_SUPPORT_SCHEMA_VERSION", + "CURRENT_RUNTIME_SUPPORT", + "FRANKA_PICK_CUBE_INTO_BOWL", + "REQUIRED_ARENA_CAPABILITY", + "REQUIRED_ARENA_COMMIT", + "TaskRequest", + "RuntimeSupportError", + "RuntimeSupportProfile", + "ArenaCompilationResult", + "ArenaIntentBridge", + "AutonomousTaskProfile", + "AutonomousValidationError", + "CompilerTraceEvent", + "GenerationConfig", + "GoalStage", + "LazyArenaIntentBridge", + "MotionBackend", + "OutputConfig", + "PlannerBackend", + "PlannerConfig", + "PickPlaceSuccessThresholds", + "CompiledTaskRequest", + "ResolvedOutputConfig", + "SpatialGoalConstraint", + "ValidationIssue", + "task_request_from_dict", + "build_arena_compilation_result", + "canonical_json", + "compile_task_request", + "extract_goal_stages", + "load_task_request", + "compile_loaded_task_request", + "sha256_json", + "validate_runtime_support", +] diff --git a/isaac_autodata_interfaces/autonomous/_validation.py b/isaac_autodata_interfaces/autonomous/_validation.py new file mode 100644 index 0000000..e516e4d --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/_validation.py @@ -0,0 +1,142 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Small dependency-free helpers for strict nested schema validation.""" + +from __future__ import annotations + +import math +from collections.abc import Iterable +from typing import Any + +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue + +FieldPath = tuple[str | int, ...] + + +class IssueCollector: + """Collect deterministic field-level validation issues before raising them together.""" + + def __init__(self) -> None: + self.issues: list[ValidationIssue] = [] + + def add(self, path: FieldPath, code: str, message: str) -> None: + """Append an issue at ``path``.""" + + self.issues.append(ValidationIssue(path=path, code=code, message=message)) + + def check_keys( + self, + value: dict[str, Any], + path: FieldPath, + *, + required: Iterable[str], + optional: Iterable[str] = (), + ) -> None: + """Report missing and unknown keys for one mapping.""" + + required_keys = set(required) + allowed_keys = required_keys | set(optional) + for key in sorted(required_keys - set(value)): + self.add(path + (key,), "missing_field", "required field is missing") + for key in sorted(set(value) - allowed_keys): + self.add(path + (key,), "unknown_field", "field is not allowed by schema v1") + + def raise_if_any(self) -> None: + """Raise :class:`AutonomousValidationError` when any issues were collected.""" + + if self.issues: + raise AutonomousValidationError(self.issues) + + +def require_mapping(value: Any, path: FieldPath, issues: IssueCollector) -> dict[str, Any] | None: + """Return ``value`` as a string-keyed mapping or report an exact-type error.""" + + if type(value) is not dict: + issues.add(path, "invalid_type", f"expected mapping, got {type(value).__name__}") + return None + invalid_keys = [key for key in value if type(key) is not str] + if invalid_keys: + issues.add(path, "invalid_mapping_key", "mapping keys must be strings") + return None + return value + + +def require_string( + value: Any, + path: FieldPath, + issues: IssueCollector, + *, + allow_empty: bool = False, +) -> str | None: + """Return a string while rejecting non-strings and blank identifiers.""" + + if type(value) is not str: + issues.add(path, "invalid_type", f"expected string, got {type(value).__name__}") + return None + if not allow_empty and not value.strip(): + issues.add(path, "empty_value", "value must not be empty or whitespace-only") + return None + if "\x00" in value: + issues.add(path, "invalid_value", "value must not contain a NUL character") + return None + return value + + +def require_bool(value: Any, path: FieldPath, issues: IssueCollector) -> bool | None: + """Return an exact YAML boolean; integers and truthy strings are rejected.""" + + if type(value) is not bool: + issues.add(path, "invalid_type", f"expected boolean, got {type(value).__name__}") + return None + return value + + +def require_int( + value: Any, + path: FieldPath, + issues: IssueCollector, + *, + minimum: int | None = None, + maximum: int | None = None, +) -> int | None: + """Return an exact integer with optional inclusive bounds.""" + + if type(value) is not int: + issues.add(path, "invalid_type", f"expected integer, got {type(value).__name__}") + return None + if minimum is not None and value < minimum: + issues.add(path, "out_of_range", f"value must be at least {minimum}, got {value}") + return None + if maximum is not None and value > maximum: + issues.add(path, "out_of_range", f"value must be at most {maximum}, got {value}") + return None + return value + + +def require_finite_number( + value: Any, + path: FieldPath, + issues: IssueCollector, + *, + minimum_exclusive: float | None = None, + maximum: float | None = None, +) -> float | None: + """Return a finite exact int/float value, excluding booleans.""" + + if type(value) not in (int, float): + issues.add(path, "invalid_type", f"expected number, got {type(value).__name__}") + return None + result = float(value) + if not math.isfinite(result): + issues.add(path, "non_finite", "value must be finite") + return None + if minimum_exclusive is not None and result <= minimum_exclusive: + issues.add(path, "out_of_range", f"value must be greater than {minimum_exclusive:g}, got {value}") + return None + if maximum is not None and result > maximum: + issues.add(path, "out_of_range", f"value must be at most {maximum:g}, got {value}") + return None + return result diff --git a/isaac_autodata_interfaces/autonomous/_yaml.py b/isaac_autodata_interfaces/autonomous/_yaml.py new file mode 100644 index 0000000..d0e8be2 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/_yaml.py @@ -0,0 +1,161 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Strict, duplicate-safe YAML loading for autonomous configuration.""" + +from __future__ import annotations + +import re +import yaml +from pathlib import Path +from typing import Any +from yaml.nodes import MappingNode, Node, ScalarNode, SequenceNode + +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue + +MAX_YAML_BYTES = 1_000_000 +"""Maximum UTF-8 document size accepted by the offline compiler.""" + + +class _TaskSafeLoader(yaml.SafeLoader): + """Safe loader with YAML 1.2 boolean semantics. + + PyYAML's default YAML 1.1 resolver treats robotics relation names such as ``on`` and ``off`` + as booleans. Task request files use those words as Arena semantic strings, so only the YAML + 1.2 spellings ``true`` and ``false`` are resolved as booleans here. + """ + + +_TaskSafeLoader.yaml_implicit_resolvers = { + key: list(resolvers) for key, resolvers in yaml.SafeLoader.yaml_implicit_resolvers.items() +} +for first_character, resolvers in _TaskSafeLoader.yaml_implicit_resolvers.items(): + _TaskSafeLoader.yaml_implicit_resolvers[first_character] = [ + (tag, regexp) for tag, regexp in resolvers if tag != "tag:yaml.org,2002:bool" + ] +_TaskSafeLoader.add_implicit_resolver( + "tag:yaml.org,2002:bool", + re.compile(r"^(?:true|True|TRUE|false|False|FALSE)$"), + list("tTfF"), +) + + +def load_yaml_document(path: str | Path) -> Any: + """Load one safe YAML document while retaining duplicate-key detection and field paths. + + Args: + path: YAML file to load. + + Returns: + Nested Python scalars, lists, and dictionaries. + + Raises: + AutonomousValidationError: If the file cannot be read, exceeds the size limit, contains + invalid YAML, duplicate/complex mapping keys, recursive aliases, or multiple documents. + """ + + source_path = Path(path).expanduser() + try: + raw = source_path.read_bytes() + except FileNotFoundError: + raise AutonomousValidationError( + [ValidationIssue((), "file_not_found", f"YAML file does not exist: {source_path}")] + ) from None + except OSError as exc: + raise AutonomousValidationError( + [ValidationIssue((), "file_read_error", f"could not read YAML file: {exc}")] + ) from None + + if len(raw) > MAX_YAML_BYTES: + raise AutonomousValidationError([ + ValidationIssue( + (), + "document_too_large", + f"YAML document is {len(raw)} bytes; maximum is {MAX_YAML_BYTES}", + ) + ]) + try: + text = raw.decode("utf-8") + except UnicodeDecodeError as exc: + raise AutonomousValidationError( + [ValidationIssue((), "invalid_encoding", f"YAML must be UTF-8: {exc}")] + ) from None + return _load_yaml_text(text) + + +def _load_yaml_text(text: str) -> Any: + loader = _TaskSafeLoader(text) + try: + node = loader.get_single_node() + if node is None: + raise AutonomousValidationError([ValidationIssue((), "empty_document", "YAML document is empty")]) + return _construct_node(loader, node, (), set(), set()) + except AutonomousValidationError: + raise + except yaml.YAMLError as exc: + mark = getattr(exc, "problem_mark", None) + location = "" + if mark is not None: + location = f" at line {mark.line + 1}, column {mark.column + 1}" + problem = getattr(exc, "problem", None) or str(exc).splitlines()[0] + raise AutonomousValidationError( + [ValidationIssue((), "yaml_syntax", f"invalid YAML{location}: {problem}")] + ) from None + finally: + loader.dispose() + + +def _construct_node( + loader: yaml.SafeLoader, + node: Node, + path: tuple[str | int, ...], + active_nodes: set[int], + seen_nodes: set[int], +) -> Any: + node_id = id(node) + if node_id in active_nodes: + raise AutonomousValidationError( + [ValidationIssue(path, "recursive_alias", "recursive YAML aliases are not allowed")] + ) + if node_id in seen_nodes: + raise AutonomousValidationError( + [ + ValidationIssue( + path, + "yaml_alias_not_allowed", + "YAML aliases are not allowed in task requests", + ) + ] + ) + seen_nodes.add(node_id) + active_nodes.add(node_id) + try: + if isinstance(node, MappingNode): + result: dict[str, Any] = {} + for key_node, value_node in node.value: + if not isinstance(key_node, ScalarNode) or key_node.tag != "tag:yaml.org,2002:str": + raise AutonomousValidationError( + [ValidationIssue(path, "invalid_mapping_key", "mapping keys must be strings")] + ) + key = key_node.value + key_path = path + (key,) + if key in result: + raise AutonomousValidationError( + [ValidationIssue(key_path, "duplicate_key", f"mapping key {key!r} is duplicated")] + ) + result[key] = _construct_node(loader, value_node, key_path, active_nodes, seen_nodes) + return result + if isinstance(node, SequenceNode): + return [ + _construct_node(loader, child, path + (index,), active_nodes, seen_nodes) + for index, child in enumerate(node.value) + ] + if isinstance(node, ScalarNode): + return loader.construct_object(node, deep=True) + raise AutonomousValidationError( + [ValidationIssue(path, "unsupported_yaml_node", f"unsupported YAML node {type(node).__name__}")] + ) + finally: + active_nodes.remove(node_id) diff --git a/isaac_autodata_interfaces/autonomous/arena_bridge.py b/isaac_autodata_interfaces/autonomous/arena_bridge.py new file mode 100644 index 0000000..e3402da --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/arena_bridge.py @@ -0,0 +1,347 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Lazy bridge to Arena's authoritative environment-intent API.""" + +from __future__ import annotations + +import importlib +import json +import math +import random +from collections.abc import Callable, Mapping, Sequence +from typing import Any, Protocol + +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.task_request_types import ( + ArenaCompilationResult, + CompilerTraceEvent, + GoalStage, + SpatialGoalConstraint, + canonical_json, + sha256_json, +) + +REQUIRED_ARENA_COMMIT = "8a74e794b621b0f8d3627d096a1bae9ce11e7b56" +REQUIRED_ARENA_CAPABILITY = ( + "EnvironmentIntentSpec.model_validate + IntentCompiler.compile/resolution_errors + " + "ArenaEnvInitialGraphSpec.link/to_dict" +) +MAX_SAFE_TEXT_LENGTH = 2048 + + +class ArenaIntentBridge(Protocol): + """Interface accepted by the pure AutoData task compiler and fake-backed tests.""" + + def compile_and_link(self, intent: dict[str, Any], *, seed: int) -> ArenaCompilationResult: + """Validate, compile, and link one opaque Arena environment intent.""" + + +class LazyArenaIntentBridge: + """Import and invoke Arena's intent API only when resolution is explicitly requested.""" + + def __init__(self, module_loader: Callable[[str], Any] = importlib.import_module) -> None: + self._module_loader = module_loader + + def compile_and_link(self, intent: dict[str, Any], *, seed: int) -> ArenaCompilationResult: + """Validate, compile, and link an Arena intent with isolated deterministic randomness.""" + + random_state = random.getstate() + try: + random.seed(seed) + environment_intent_cls, compiler_cls = self._load_api() + try: + intent_spec = environment_intent_cls.model_validate(intent) + except Exception as exc: + raise AutonomousValidationError([ + ValidationIssue( + ("environment", "intent"), + "arena_intent_invalid", + f"Arena EnvironmentIntentSpec rejected the intent: {_safe_exception(exc)}", + ) + ]) from None + + try: + compiler = compiler_cls() + initial_graph_model = compiler.compile(intent_spec) + except AutonomousValidationError: + raise + except Exception as exc: + raise AutonomousValidationError([ + ValidationIssue( + ("environment", "intent"), + "arena_compile_failed", + f"Arena IntentCompiler failed: {_safe_exception(exc)}", + ) + ]) from None + + if not hasattr(compiler, "resolution_errors") or not hasattr(compiler, "trace"): + raise _arena_api_error("IntentCompiler lacks resolution_errors or trace after compile") + resolution_errors = list(compiler.resolution_errors) + if resolution_errors: + issues = [] + for event in resolution_errors: + trace_event = _trace_event_to_plain(event) + issues.append( + ValidationIssue( + ("environment", "intent"), + "arena_resolution_error", + f"Arena resolution stage {trace_event.stage!r} could not resolve " + f"{trace_event.query!r}; chosen={trace_event.chosen!r}; note={trace_event.note!r}", + ) + ) + raise AutonomousValidationError(issues) + + if not hasattr(initial_graph_model, "link"): + raise _arena_api_error("compiled initial graph lacks link()") + try: + linked_graph_model = initial_graph_model.link() + except Exception as exc: + raise AutonomousValidationError([ + ValidationIssue( + ("environment", "intent"), + "arena_link_failed", + f"Arena initial graph linking failed: {_safe_exception(exc)}", + ) + ]) from None + + initial_graph = _graph_model_to_dict(initial_graph_model, "initial") + linked_graph = _graph_model_to_dict(linked_graph_model, "linked") + trace = tuple(_trace_event_to_plain(event) for event in compiler.trace) + return build_arena_compilation_result(initial_graph, linked_graph, trace) + finally: + random.setstate(random_state) + + def is_available(self) -> bool: + """Return whether the required Arena intent API can be imported in this process.""" + + try: + self._load_api() + except AutonomousValidationError: + return False + return True + + def _load_api(self) -> tuple[type, type]: + try: + intent_module = self._module_loader("isaaclab_arena.agentic_environment_generation.environment_intent_spec") + compiler_module = self._module_loader("isaaclab_arena.agentic_environment_generation.intent_compiler") + environment_intent_cls = getattr(intent_module, "EnvironmentIntentSpec") + compiler_cls = getattr(compiler_module, "IntentCompiler") + except Exception as exc: + raise _arena_api_error(_safe_exception(exc)) from None + if not callable(getattr(environment_intent_cls, "model_validate", None)): + raise _arena_api_error("EnvironmentIntentSpec lacks model_validate()") + if not callable(getattr(compiler_cls, "compile", None)): + raise _arena_api_error("IntentCompiler lacks compile()") + return environment_intent_cls, compiler_cls + + +def build_arena_compilation_result( + initial_graph: Mapping[str, Any], + linked_graph: Mapping[str, Any], + compiler_trace: Sequence[CompilerTraceEvent | Mapping[str, Any]] = (), +) -> ArenaCompilationResult: + """Validate plain Arena artifacts and derive their digest and ordered spatial goal stages. + + This helper is also the supported construction path for fake bridges in pure tests. + """ + + initial_plain = _plain_mapping(initial_graph, ("arena", "initial_graph")) + linked_plain = _plain_mapping(linked_graph, ("arena", "linked_graph")) + trace = tuple(_trace_event_to_plain(event) for event in compiler_trace) + goal_stages = extract_goal_stages(linked_plain) + return ArenaCompilationResult( + initial_graph_json=canonical_json(initial_plain), + linked_graph_json=canonical_json(linked_plain), + compiler_trace=trace, + graph_digest=sha256_json(linked_plain), + goal_stages=goal_stages, + ) + + +def extract_goal_stages(linked_graph: Mapping[str, Any]) -> tuple[GoalStage, ...]: + """Extract each linked task's success-state spatial constraints in task order.""" + + graph = _plain_mapping(linked_graph, ("arena", "linked_graph")) + tasks = graph.get("tasks") + state_specs = graph.get("state_specs") + if type(tasks) is not list: + raise _graph_issue(("arena", "linked_graph", "tasks"), "expected a task list") + if type(state_specs) is not list: + raise _graph_issue(("arena", "linked_graph", "state_specs"), "expected a state-spec list") + + states_by_id: dict[str, dict[str, Any]] = {} + for index, value in enumerate(state_specs): + path = ("arena", "linked_graph", "state_specs", index) + state = _require_plain_dict(value, path) + state_id = _require_plain_string(state.get("id"), path + ("id",)) + if state_id in states_by_id: + raise _graph_issue(path + ("id",), f"duplicate state id {state_id!r}") + states_by_id[state_id] = state + + stages: list[GoalStage] = [] + for index, value in enumerate(tasks): + task_path = ("arena", "linked_graph", "tasks", index) + task = _require_plain_dict(value, task_path) + task_id = _require_plain_string(task.get("id"), task_path + ("id",)) + task_kind = _require_plain_string(task.get("kind"), task_path + ("kind",)) + success_id = _require_plain_string(task.get("success_state_spec_id"), task_path + ("success_state_spec_id",)) + if success_id not in states_by_id: + raise _graph_issue( + task_path + ("success_state_spec_id",), + f"task references missing success state {success_id!r}", + ) + state = states_by_id[success_id] + constraints = state.get("spatial_constraints", []) + if type(constraints) is not list: + raise _graph_issue( + ("arena", "linked_graph", "state_specs", success_id, "spatial_constraints"), + "expected a spatial-constraint list", + ) + typed_constraints = tuple( + _parse_spatial_constraint(constraint, task_path + ("success_state", "spatial_constraints", offset)) + for offset, constraint in enumerate(constraints) + ) + stages.append( + GoalStage( + index=index, + task_id=task_id, + task_kind=task_kind, + success_state_spec_id=success_id, + spatial_constraints=typed_constraints, + ) + ) + return tuple(stages) + + +def _parse_spatial_constraint(value: Any, path: tuple[str | int, ...]) -> SpatialGoalConstraint: + constraint = _require_plain_dict(value, path) + constraint_id = _require_plain_string(constraint.get("id"), path + ("id",)) + kind = _require_plain_string(constraint.get("kind"), path + ("kind",)) + subject = _require_plain_string(constraint.get("subject"), path + ("subject",)) + reference_value = constraint.get("reference") + if reference_value is not None and type(reference_value) is not str: + raise _graph_issue(path + ("reference",), "expected string or null") + params = constraint.get("params", {}) + params_plain = _require_plain_dict(params, path + ("params",)) + return SpatialGoalConstraint( + id=constraint_id, + kind=kind, + subject=subject, + reference=reference_value, + params_json=canonical_json(params_plain), + ) + + +def _graph_model_to_dict(model: Any, graph_kind: str) -> dict[str, Any]: + try: + if callable(getattr(model, "to_dict", None)): + value = model.to_dict() + elif callable(getattr(model, "model_dump", None)): + value = model.model_dump(mode="json", exclude_none=True) + else: + raise TypeError(f"{type(model).__name__} lacks to_dict() and model_dump()") + return _plain_mapping(value, ("arena", f"{graph_kind}_graph")) + except AutonomousValidationError: + raise + except Exception as exc: + raise AutonomousValidationError([ + ValidationIssue( + ("arena", f"{graph_kind}_graph"), + "arena_graph_serialization_failed", + f"Arena {graph_kind} graph could not be serialized: {_safe_exception(exc)}", + ) + ]) from None + + +def _plain_mapping(value: Mapping[str, Any], path: tuple[str | int, ...]) -> dict[str, Any]: + if not isinstance(value, Mapping): + raise _graph_issue(path, f"expected mapping, got {type(value).__name__}") + plain = dict(value) + _validate_plain_json(plain, path) + return json.loads(canonical_json(plain)) + + +def _validate_plain_json(value: Any, path: tuple[str | int, ...]) -> None: + stack: list[tuple[Any, tuple[str | int, ...]]] = [(value, path)] + while stack: + current, current_path = stack.pop() + if current is None or type(current) in (str, bool, int): + continue + if type(current) is float: + if not math.isfinite(current): + raise _graph_issue(current_path, "number must be finite") + continue + if type(current) is list: + stack.extend((item, current_path + (index,)) for index, item in enumerate(current)) + continue + if type(current) is dict: + if any(type(key) is not str for key in current): + raise _graph_issue(current_path, "mapping keys must be strings") + stack.extend((item, current_path + (key,)) for key, item in current.items()) + continue + raise _graph_issue(current_path, f"expected JSON-compatible value, got {type(current).__name__}") + + +def _trace_event_to_plain(value: CompilerTraceEvent | Mapping[str, Any] | Any) -> CompilerTraceEvent: + if isinstance(value, CompilerTraceEvent): + return value + if isinstance(value, Mapping): + stage = value.get("stage") + query = value.get("query") + chosen = value.get("chosen") + note = value.get("note", "") + else: + stage = getattr(value, "stage", None) + query = getattr(value, "query", None) + chosen = getattr(value, "chosen", None) + note = getattr(value, "note", "") + if type(stage) is not str or type(query) is not str or type(note) is not str: + raise _graph_issue(("arena", "compiler_trace"), "trace stage, query, and note must be strings") + if chosen is not None and type(chosen) is not str: + raise _graph_issue(("arena", "compiler_trace"), "trace chosen must be a string or null") + return CompilerTraceEvent( + stage=_bounded_text(stage), + query=_bounded_text(query), + chosen=None if chosen is None else _bounded_text(chosen), + note=_bounded_text(note), + ) + + +def _require_plain_dict(value: Any, path: tuple[str | int, ...]) -> dict[str, Any]: + if type(value) is not dict: + raise _graph_issue(path, f"expected mapping, got {type(value).__name__}") + return value + + +def _require_plain_string(value: Any, path: tuple[str | int, ...]) -> str: + if type(value) is not str or not value: + raise _graph_issue(path, "expected non-empty string") + return value + + +def _graph_issue(path: tuple[str | int, ...], message: str) -> AutonomousValidationError: + return AutonomousValidationError([ValidationIssue(path, "arena_graph_contract_error", message)]) + + +def _arena_api_error(detail: str) -> AutonomousValidationError: + message = ( + f"Arena agentic intent API is unavailable or incompatible ({detail}). Required capability:" + f" {REQUIRED_ARENA_CAPABILITY}. Use IsaacLab-Arena commit {REQUIRED_ARENA_COMMIT} or a reviewed compatible" + " commit." + ) + return AutonomousValidationError( + [ValidationIssue(("environment", "intent"), "arena_intent_api_unavailable", message)] + ) + + +def _safe_exception(exc: Exception) -> str: + return _bounded_text(f"{type(exc).__name__}: {exc}") + + +def _bounded_text(value: str) -> str: + if len(value) <= MAX_SAFE_TEXT_LENGTH: + return value + return value[: MAX_SAFE_TEXT_LENGTH - 3] + "..." diff --git a/isaac_autodata_interfaces/autonomous/arena_environment.py b/isaac_autodata_interfaces/autonomous/arena_environment.py new file mode 100644 index 0000000..4508096 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/arena_environment.py @@ -0,0 +1,666 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Materialize a resolved Arena intent as a recordable autonomous environment.""" + +from __future__ import annotations + +import hashlib +import math +import os +import stat +import urllib.error +import urllib.request +from collections.abc import Callable, Mapping +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +from isaac_autodata_core.autonomous.output_transaction import RecordingTargets +from isaac_autodata_core.autonomous.task_motion import GoalPredicate +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import FRANKA_PICK_CUBE_INTO_BOWL +from isaac_autodata_interfaces.autonomous.task_request_types import CompiledTaskRequest + +_TASK_PROFILE = FRANKA_PICK_CUBE_INTO_BOWL +_RUNTIME_USD_READ_CHUNK_BYTES = 64 << 10 +_RUNTIME_USD_TIMEOUT_S = 10.0 + +RuntimeUsdAttestor = Callable[[str], Mapping[str, Any]] + + +@dataclass +class ArenaRuntimeBundle: + """Live resources created from a compiled task request after SimulationApp launch.""" + + env: Any + embodiment_adapter: Any + success_term: Any + graph_spec: Any + step_dt_s: float + success_contract_evidence: dict[str, Any] = field(default_factory=dict) + runtime_asset_evidence: dict[str, Any] = field(default_factory=dict) + _closed: bool = field(default=False, init=False, repr=False) + + def close(self) -> None: + """Close the live environment.""" + + if self._closed: + return + self._closed = True + self.env.close() + + +def goal_predicates_from_request(request: CompiledTaskRequest) -> tuple[GoalPredicate, ...]: + """Project Arena's linked task-stage constraints into planner-neutral goal predicates.""" + + predicates: list[GoalPredicate] = [] + for stage in request.goal_stages: + for constraint in stage.spatial_constraints: + predicates.append( + GoalPredicate( + relation=constraint.kind, + subject=constraint.subject, + target=constraint.reference, + ) + ) + if not predicates: + raise ValueError("resolved Arena task graph contains no spatial success constraints") + return tuple(predicates) + + +def make_embodiment_adapter(request: CompiledTaskRequest) -> Any: + """Create the reviewed AutoData controller binding for the resolved Arena embodiment. + + Arena remains authoritative for semantic asset selection. This binding only describes the + live action/observation layout needed to execute planner poses. The prototype intentionally + supports the registered ``franka_ik`` profile and fails explicitly for all others. + """ + + nodes = request.linked_graph.get("nodes", []) + embodiment_nodes = [node for node in nodes if node.get("type") == "embodiment"] + if len(embodiment_nodes) != 1: + raise ValueError(f"autonomous generation requires exactly one embodiment node, got {len(embodiment_nodes)}") + embodiment_name = embodiment_nodes[0].get("name") + if embodiment_name != _TASK_PROFILE.embodiment_name: + raise NotImplementedError( + f"autonomous runtime support for Arena embodiment {embodiment_name!r} is not available; " + f"the prototype currently supports {_TASK_PROFILE.embodiment_name!r}" + ) + + from isaac_autodata_interfaces.embodiments.embodiment_types import PoseObsKeys + from isaac_autodata_interfaces.embodiments.single_arm_embodiment_adapter import DeltaPoseIKSingleArmAdapter + + return DeltaPoseIKSingleArmAdapter( + name="arena_franka_ik", + description="Arena Franka IK relative-pose runtime binding", + eef_name="franka", + pose_obs_keys=PoseObsKeys(pos="eef_pos", quat="eef_quat"), + gripper_action_dim=1, + obs_group="policy", + # Arena observes panda_hand + 0.1034 m while its DIK command frame is panda_hand + + # 0.107 m. ``eef_offset`` is command-to-observation, so -3.6 mm makes the adapter + # report the exact command frame. The live provider independently attests this against + # the instantiated action term and fails closed if Arena changes either frame. + eef_offset=_TASK_PROFILE.command_to_observation_offset_m, + ) + + +class _RejectRedirectHandler(urllib.request.HTTPRedirectHandler): + """Disable urllib's default redirect following for pinned asset requests.""" + + def redirect_request(self, req: Any, fp: Any, code: int, msg: str, headers: Any, newurl: str) -> None: + del req, fp, code, msg, headers, newurl + + +def _open_runtime_usd_request(request: urllib.request.Request, timeout_s: float) -> Any: + """Open one HTTPS request with redirects disabled and platform TLS verification enabled.""" + + opener = urllib.request.build_opener(_RejectRedirectHandler()) + return opener.open(request, timeout=timeout_s) + + +def _validate_content_identity_inputs( + expected_sha256: str, + expected_bytes: int, + max_bytes: int, + timeout_s: float, +) -> None: + """Validate bounded exact-content attestation inputs.""" + + if ( + isinstance(expected_bytes, bool) + or not isinstance(expected_bytes, int) + or expected_bytes <= 0 + or expected_bytes > max_bytes + ): + raise ValueError("runtime USD expected byte count must be positive and within the download bound") + if ( + isinstance(max_bytes, bool) + or not isinstance(max_bytes, int) + or max_bytes <= 0 + or max_bytes > _TASK_PROFILE.runtime_usd_max_bytes + ): + raise ValueError(f"runtime USD download bound must be in [1, {_TASK_PROFILE.runtime_usd_max_bytes}]") + if not isinstance(expected_sha256, str) or len(expected_sha256) != 64: + raise ValueError("runtime USD expected SHA-256 must contain 64 hexadecimal characters") + try: + bytes.fromhex(expected_sha256) + except ValueError as exc: + raise ValueError("runtime USD expected SHA-256 must contain 64 hexadecimal characters") from exc + if not math.isfinite(timeout_s) or not 0 < timeout_s <= 30: + raise ValueError("runtime USD timeout must be finite and in (0, 30] seconds") + + +def _request_exact_url_content( + url: str, + timeout_s: float, + opener: Callable[[urllib.request.Request, float], Any] | None, +) -> Any: + """Issue one identity-encoded request and normalize transport failures.""" + + request = urllib.request.Request( + url, + headers={ + "Accept": "application/octet-stream,*/*;q=0.1", + "Accept-Encoding": "identity", + "User-Agent": "Isaac-AutoData-root-layer-attestor/1", + }, + method="GET", + ) + open_request = _open_runtime_usd_request if opener is None else opener + try: + return open_request(request, timeout_s) + except urllib.error.HTTPError as exc: + if 300 <= exc.code < 400: + raise ValueError(f"runtime USD redirects are forbidden (HTTP {exc.code})") from exc + raise ValueError(f"runtime USD root layer request failed with HTTP {exc.code}") from exc + except (urllib.error.URLError, TimeoutError, OSError) as exc: + raise ValueError(f"runtime USD root layer request failed: {type(exc).__name__}: {str(exc)[:256]}") from exc + except Exception as exc: + raise ValueError(f"runtime USD root layer request failed: {type(exc).__name__}: {str(exc)[:256]}") from exc + + +def _validate_content_response_headers(headers: Any, max_bytes: int) -> None: + """Reject encoded, malformed, or declared-oversize content.""" + + if headers is None or not callable(getattr(headers, "get", None)): + raise ValueError("runtime USD root layer response has no inspectable headers") + content_encoding = headers.get("Content-Encoding") + if content_encoding is not None and str(content_encoding).strip().lower() not in ("", "identity"): + raise ValueError("runtime USD root layer response must use identity content encoding") + content_length = headers.get("Content-Length") + if content_length is None: + return + try: + declared_bytes = int(content_length) + except (TypeError, ValueError) as exc: + raise ValueError("runtime USD root layer Content-Length is invalid") from exc + if declared_bytes < 0: + raise ValueError("runtime USD root layer Content-Length is invalid") + if declared_bytes > max_bytes: + raise ValueError(f"runtime USD root layer exceeds the {max_bytes}-byte download bound") + + +def _validate_content_response_identity(response: Any, expected_url: str, max_bytes: int) -> str: + """Attest response status, final URL, encoding, and declared size.""" + + status = getattr(response, "status", None) + if status is None and callable(getattr(response, "getcode", None)): + status = response.getcode() + if type(status) is not int or status != 200: + raise ValueError(f"runtime USD root layer request returned non-success status {status!r}") + final_url = response.geturl() if callable(getattr(response, "geturl", None)) else None + if final_url != expected_url: + raise ValueError("runtime USD redirects or final-URL changes are forbidden") + _validate_content_response_headers(getattr(response, "headers", None), max_bytes) + return final_url + + +def _read_bounded_content(response: Any, max_bytes: int) -> bytes: + """Read at most one byte beyond the configured ceiling to detect streamed overflow.""" + + payload = bytearray() + while True: + chunk = response.read(min(_RUNTIME_USD_READ_CHUNK_BYTES, max_bytes + 1 - len(payload))) + if not chunk: + return bytes(payload) + if not isinstance(chunk, (bytes, bytearray)): + raise ValueError("runtime USD root layer response returned non-byte content") + payload.extend(chunk) + if len(payload) > max_bytes: + raise ValueError(f"runtime USD root layer exceeds the {max_bytes}-byte download bound") + + +def _consume_exact_url_response(response: Any, expected_url: str, max_bytes: int) -> tuple[bytes, str]: + """Validate and consume one response while normalizing bounded read failures.""" + + try: + with response as opened_response: + final_url = _validate_content_response_identity(opened_response, expected_url, max_bytes) + return _read_bounded_content(opened_response, max_bytes), final_url + except ValueError: + raise + except (urllib.error.URLError, TimeoutError, OSError) as exc: + raise ValueError(f"runtime USD root layer read failed: {type(exc).__name__}: {str(exc)[:256]}") from exc + except Exception as exc: + raise ValueError(f"runtime USD root layer read failed: {type(exc).__name__}: {str(exc)[:256]}") from exc + + +def _attest_exact_url_content( + url: str, + *, + expected_url: str, + expected_sha256: str, + expected_bytes: int, + max_bytes: int, + timeout_s: float, + opener: Callable[[urllib.request.Request, float], Any] | None = None, +) -> dict[str, Any]: + """Fetch and hash one exact HTTPS object under strict redirect and byte bounds.""" + + if url != expected_url or not expected_url.startswith("https://"): + raise ValueError("runtime USD root layer URL must equal the pinned HTTPS production URL") + _validate_content_identity_inputs(expected_sha256, expected_bytes, max_bytes, timeout_s) + response = _request_exact_url_content(url, timeout_s, opener) + payload, final_url = _consume_exact_url_response(response, expected_url, max_bytes) + + actual_bytes = len(payload) + actual_sha256 = hashlib.sha256(payload).hexdigest() + if actual_bytes != expected_bytes: + raise ValueError( + f"runtime USD root layer byte count mismatch: expected {expected_bytes}, observed {actual_bytes}" + ) + if actual_sha256 != expected_sha256: + raise ValueError("runtime USD root layer SHA-256 does not match the reviewed Isaac 5.1 object") + return { + "attestation_method": "https_exact_url_sha256_v1", + "attested": True, + "bytes": actual_bytes, + "content_encoding": "identity", + "expected_bytes": expected_bytes, + "expected_sha256": expected_sha256, + "final_url": final_url, + "http_status": 200, + "max_bytes": max_bytes, + "redirects_allowed": False, + "scope": "root_layer_bytes_only", + "sha256": actual_sha256, + "url": url, + } + + +def attest_custream_v1_runtime_usd_root( + runtime_usd_path: str, + *, + opener: Callable[[urllib.request.Request, float], Any] | None = None, +) -> dict[str, Any]: + """Attest the exact official Isaac 5.1 Panda root-layer bytes. + + This attestation covers only the bytes returned for ``panda_instanceable.usd``. Referenced USD + dependencies are not fetched or attested here. Robot kinematics and command/observation frame + compatibility remain a separate fail-closed live-provider attestation. + + Args: + runtime_usd_path: Exact pinned official-production HTTPS URL. + opener: Optional injected request opener used by bounded offline unit tests. + + Returns: + JSON-compatible root-layer content identity evidence. + + Raises: + ValueError: If the URL, HTTP response, byte bound, size, or digest does not match. + """ + + return _attest_exact_url_content( + runtime_usd_path, + expected_url=_TASK_PROFILE.runtime_usd_path, + expected_sha256=_TASK_PROFILE.runtime_usd_sha256, + expected_bytes=_TASK_PROFILE.runtime_usd_bytes, + max_bytes=_TASK_PROFILE.runtime_usd_max_bytes, + timeout_s=_RUNTIME_USD_TIMEOUT_S, + opener=opener, + ) + + +def _validated_runtime_usd_root_evidence(value: Mapping[str, Any]) -> dict[str, Any]: + """Validate and bound evidence returned by an injected or production root-layer attestor.""" + + expected = { + "attestation_method": "https_exact_url_sha256_v1", + "attested": True, + "bytes": _TASK_PROFILE.runtime_usd_bytes, + "content_encoding": "identity", + "expected_bytes": _TASK_PROFILE.runtime_usd_bytes, + "expected_sha256": _TASK_PROFILE.runtime_usd_sha256, + "final_url": _TASK_PROFILE.runtime_usd_path, + "http_status": 200, + "max_bytes": _TASK_PROFILE.runtime_usd_max_bytes, + "redirects_allowed": False, + "scope": "root_layer_bytes_only", + "sha256": _TASK_PROFILE.runtime_usd_sha256, + "url": _TASK_PROFILE.runtime_usd_path, + } + if not isinstance(value, Mapping) or any(value.get(key) != item for key, item in expected.items()): + raise ValueError("runtime USD root-layer attestor returned incomplete or mismatched identity evidence") + return expected + + +def apply_custream_v1_runtime_asset_profile( + request: CompiledTaskRequest, + scene_cfg: Any, + *, + runtime_usd_path: str, + root_layer_attestor: RuntimeUsdAttestor | None = None, +) -> dict[str, Any]: + """Select the reviewed runtime USD and attest only its official root-layer identity. + + Arena's registered ``franka_ik`` embodiment currently uses a combined robot-and-stand USD. + This profile changes only the composed articulation's ``spawn.usd_path`` to the pinned official + Isaac 5.1 Panda root layer. The content attestation does not cover referenced USD dependencies + and makes no claim of URDF/USD kinematic equivalence. Live kinematic and action-frame agreement + must be attested independently by the motion-provider boundary. + + Args: + request: Fully linked semantic request admitted by the live v1 runtime-support profile. + scene_cfg: Composed Arena scene configuration, before environment construction. + runtime_usd_path: Exact official-production Panda USD selected by this reviewed profile. + root_layer_attestor: Optional injected exact-content attestor for bounded unit tests. + + Returns: + JSON-compatible evidence describing the exact reviewed runtime substitution. + + Raises: + ValueError: If the semantic embodiment, asset path, or root-layer identity has drifted. + """ + + linked_graph = getattr(request, "linked_graph", None) + nodes = linked_graph.get("nodes") if isinstance(linked_graph, dict) else None + if not isinstance(nodes, list): + raise ValueError("custream v1 runtime asset profile requires a linked Arena node list") + embodiment_names = [ + node.get("name") for node in nodes if isinstance(node, dict) and node.get("type") == "embodiment" + ] + if embodiment_names != [_TASK_PROFILE.embodiment_name]: + raise ValueError( + "custream v1 runtime asset profile requires exactly the Arena 'franka_ik' embodiment; " + f"got {embodiment_names}" + ) + + robot_cfg = getattr(scene_cfg, "robot", None) + spawn_cfg = None if robot_cfg is None else getattr(robot_cfg, "spawn", None) + registry_usd_path = None if spawn_cfg is None else getattr(spawn_cfg, "usd_path", None) + if not isinstance(registry_usd_path, str) or not registry_usd_path: + raise ValueError("composed Arena franka_ik scene has no robot spawn USD path") + registry_usd_basename = os.path.basename(registry_usd_path) + if registry_usd_basename != _TASK_PROFILE.registry_usd_basename: + raise ValueError( + "Arena franka_ik registry asset changed from the reviewed custream v1 source " + f"{_TASK_PROFILE.registry_usd_basename!r} to {registry_usd_basename!r}" + ) + if not isinstance(runtime_usd_path, str) or not runtime_usd_path: + raise ValueError("live FRANKA_PANDA_HIGH_PD_CFG has no runtime USD path") + runtime_usd_basename = os.path.basename(runtime_usd_path) + if runtime_usd_basename != _TASK_PROFILE.runtime_usd_basename: + raise ValueError( + "IsaacLab Franka runtime asset changed from the reviewed custream v1 target " + f"{_TASK_PROFILE.runtime_usd_basename!r} to {runtime_usd_basename!r}" + ) + if runtime_usd_path != _TASK_PROFILE.runtime_usd_path: + raise ValueError("custream v1 runtime asset must use the reviewed pinned official production URI") + if registry_usd_path == runtime_usd_path: + raise ValueError("custream v1 runtime asset substitution unexpectedly resolves to the registry asset") + + attest_root_layer = attest_custream_v1_runtime_usd_root if root_layer_attestor is None else root_layer_attestor + try: + raw_root_layer_evidence = attest_root_layer(runtime_usd_path) + root_layer_evidence = _validated_runtime_usd_root_evidence(raw_root_layer_evidence) + except Exception as exc: + if isinstance(exc, ValueError): + raise ValueError(f"custream v1 runtime USD root-layer attestation failed: {exc}") from exc + raise ValueError( + f"custream v1 runtime USD root-layer attestation failed: {type(exc).__name__}: {str(exc)[:256]}" + ) from exc + + spawn_cfg.usd_path = runtime_usd_path + return { + "attested": True, + "attestation_scope": "runtime_usd_root_layer_identity_only", + "kinematic_frame_attestation": "separate_live_provider_attestation_required", + "motion_backend": "curobo_v1", + "override": "composed_scene.robot.spawn.usd_path_only", + "profile": _TASK_PROFILE.runtime_asset_profile, + "reason": "pinned_official_isaac_5_1_root_layer_content_identity", + "referenced_usd_dependencies_attested": False, + "registry_usd_basename": registry_usd_basename, + "registry_usd_path": registry_usd_path, + "root_layer": root_layer_evidence, + "runtime_usd_basename": runtime_usd_basename, + "runtime_usd_path": runtime_usd_path, + "runtime_usd_release": "Isaac 5.1", + "runtime_uri_policy": "pinned_exact_https_no_redirect", + "schedulestream_application": _TASK_PROFILE.schedulestream_application, + "schema_version": 2, + "semantic_embodiment": _TASK_PROFILE.embodiment_name, + } + + +def validate_planner_timing(requested_dt_s: float, environment_dt_s: float, *, tolerance_s: float = 1e-6) -> None: + """Reject planner/environment sample-rate drift until an explicit resampler is selected.""" + + for name, value in (("requested_dt_s", requested_dt_s), ("environment_dt_s", environment_dt_s)): + if not math.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be finite and positive") + if not math.isclose(requested_dt_s, environment_dt_s, rel_tol=1e-5, abs_tol=tolerance_s): + raise ValueError( + "planner interpolation_dt_s must match the Isaac environment step_dt until a reviewed " + f"resampler is enabled (planner={requested_dt_s:.9g}s, environment={environment_dt_s:.9g}s)" + ) + + +def attest_pick_and_place_success_contract( + success_term: Any, + scene_cfg: Any, + *, + pick_up_object: str, + destination_location: str, +) -> dict[str, Any]: + """Fail closed unless Arena exposes the exact reviewed contact-and-velocity success term.""" + + success_func = getattr(success_term, "func", None) + success_params = getattr(success_term, "params", None) + if ( + not callable(success_func) + or getattr(success_func, "__name__", None) != "check_success" + or getattr(success_func, "__module__", None) != "isaaclab_arena.tasks.terminations" + or not isinstance(success_params, dict) + ): + raise ValueError("Arena success term must be the reviewed check_success composition") + if set(success_params) != {"mode", "predicates"}: + raise ValueError("Arena check_success parameters changed from the reviewed contract") + mode = getattr(success_params["mode"], "value", success_params["mode"]) + predicates = success_params["predicates"] + if mode != "ALL" or not isinstance(predicates, list) or len(predicates) != 1: + raise ValueError("Arena success must combine exactly one predicate in ALL mode") + predicate = predicates[0] + predicate_func = getattr(predicate, "func", None) + predicate_params = getattr(predicate, "params", None) + if ( + not callable(predicate_func) + or getattr(predicate_func, "__name__", None) != "object_on_destination" + or getattr(predicate_func, "__module__", None) != "isaaclab_arena.tasks.terminations" + or not isinstance(predicate_params, dict) + ): + raise ValueError("Arena success predicate must be object_on_destination") + expected_keys = {"contact_sensor_cfg", "force_threshold", "object_cfg", "velocity_threshold"} + if set(predicate_params) != expected_keys: + raise ValueError("Arena object_on_destination parameters changed from the reviewed contract") + object_name = getattr(predicate_params["object_cfg"], "name", None) + contact_sensor_name = getattr(predicate_params["contact_sensor_cfg"], "name", None) + if object_name != pick_up_object or contact_sensor_name != "pick_up_object_contact_sensor": + raise ValueError("Arena success predicate does not bind the task-selected pickup object and contact sensor") + force_threshold = predicate_params["force_threshold"] + velocity_threshold = predicate_params["velocity_threshold"] + if type(force_threshold) not in (int, float) or not math.isclose( + float(force_threshold), + _TASK_PROFILE.arena_success_force_threshold_n, + abs_tol=1e-12, + ): + raise ValueError( + f"Arena success force threshold must remain exactly {_TASK_PROFILE.arena_success_force_threshold_n:g} N" + ) + if type(velocity_threshold) not in (int, float) or not math.isclose( + float(velocity_threshold), + _TASK_PROFILE.arena_success_velocity_threshold_m_s, + abs_tol=1e-12, + ): + raise ValueError( + "Arena success velocity threshold must remain exactly " + f"{_TASK_PROFILE.arena_success_velocity_threshold_m_s:g} m/s" + ) + + sensor_cfg = getattr(scene_cfg, "pick_up_object_contact_sensor", None) + sensor_prim_path = getattr(sensor_cfg, "prim_path", None) + filter_paths = getattr(sensor_cfg, "filter_prim_paths_expr", None) + expected_sensor_prim_path = f"{{ENV_REGEX_NS}}/{pick_up_object}" + expected_filter_prim_path = f"{{ENV_REGEX_NS}}/{destination_location}" + if sensor_prim_path != expected_sensor_prim_path: + raise ValueError("Arena pickup contact sensor prim path does not bind the task-selected object") + if not isinstance(filter_paths, list) or filter_paths != [expected_filter_prim_path]: + raise ValueError("Arena pickup contact sensor does not filter exactly against the destination object") + return { + "attested": True, + "contact_sensor": contact_sensor_name, + "contact_sensor_filter": filter_paths[0], + "force_threshold_n": float(force_threshold), + "mode": mode, + "predicate": "object_on_destination", + "subject": object_name, + "velocity_threshold_m_s": float(velocity_threshold), + } + + +def build_arena_runtime( + request: CompiledTaskRequest, + args_cli: Any, + *, + recording_targets: RecordingTargets | None = None, + allow_output_overwrite: bool = False, +) -> ArenaRuntimeBundle: + """Build Arena graph, configure source-free HDF5 recording, and create the live environment. + + This function must be called only after Isaac SimulationApp has launched. All heavy Isaac/Arena + imports are intentionally local. + """ + + if request.generation.num_envs != 1: + raise NotImplementedError("ScheduleStream autonomous generation currently requires generation.num_envs: 1") + dataset_path = Path(request.output.dataset) + if dataset_path.suffix.lower() != ".hdf5": + raise ValueError(f"output dataset must end in .hdf5: {dataset_path}") + if recording_targets is None: + dataset_targets = [dataset_path] + if request.output.keep_failed: + dataset_targets.append(dataset_path.with_name(f"{dataset_path.stem}_failed{dataset_path.suffix}")) + if not allow_output_overwrite: + occupied = [path for path in dataset_targets if path.exists() or path.is_symlink()] + if occupied: + targets = ", ".join(str(path) for path in occupied) + raise FileExistsError(f"refusing to overwrite existing output dataset target(s): {targets}") + dataset_path.parent.mkdir(parents=True, exist_ok=True) + dataset_export_dir_path = str(dataset_path.parent) + dataset_filename = dataset_path.stem + else: + if allow_output_overwrite: + raise ValueError("recording_targets cannot be combined with allow_output_overwrite") + if recording_targets.dataset_filename != dataset_path.stem: + raise ValueError("recording target filename must preserve the resolved dataset stem") + descriptor_prefix = "/proc/self/fd/" + if not recording_targets.dataset_export_dir_path.startswith(descriptor_prefix): + raise ValueError("recording target directory must be backed by a process file descriptor") + descriptor_text = recording_targets.dataset_export_dir_path[len(descriptor_prefix) :] + if not descriptor_text.isdigit(): + raise ValueError("recording target directory has an invalid file descriptor") + descriptor_stat = os.fstat(int(descriptor_text)) + if not stat.S_ISDIR(descriptor_stat.st_mode): + raise ValueError("recording target descriptor must refer to a directory") + dataset_export_dir_path = recording_targets.dataset_export_dir_path + dataset_filename = recording_targets.dataset_filename + + from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg + from isaaclab.managers import DatasetExportMode + from isaaclab_arena.environments.arena_env_builder import ArenaEnvBuilder + from isaaclab_arena.environments.arena_env_graph_spec import ArenaEnvGraphSpec + + args_cli.num_envs = request.generation.num_envs + args_cli.seed = request.generation.seed + args_cli.placement_seed = request.generation.seed + args_cli.mimic = False + + graph_spec = ArenaEnvGraphSpec.from_dict(request.linked_graph) + arena_env = graph_spec.to_arena_env(enable_cameras=bool(getattr(args_cli, "enable_cameras", False))) + builder = ArenaEnvBuilder(arena_env, args_cli) + env_cfg, env_kwargs = builder.compose_manager_cfg() + runtime_asset_evidence = apply_custream_v1_runtime_asset_profile( + request, + env_cfg.scene, + runtime_usd_path=_TASK_PROFILE.runtime_usd_path, + ) + + terminations = getattr(env_cfg, "terminations", None) + success_term = None if terminations is None else getattr(terminations, "success", None) + if success_term is None: + raise ValueError("resolved Arena environment does not expose a 'success' termination term") + tasks = request.linked_graph.get("tasks") + if not isinstance(tasks, list) or len(tasks) != 1 or not isinstance(tasks[0], dict): + raise ValueError("resolved Arena graph must contain exactly one task for success-term attestation") + task_params = tasks[0].get("params") + if not isinstance(task_params, dict): + raise ValueError("resolved Arena task has no linked parameters for success-term attestation") + pick_up_object = task_params.get("pick_up_object") + destination_location = task_params.get("destination_location") + if not isinstance(pick_up_object, str) or not isinstance(destination_location, str): + raise ValueError("resolved Arena task has invalid pickup/destination bindings") + success_contract_evidence = attest_pick_and_place_success_contract( + success_term, + env_cfg.scene, + pick_up_object=pick_up_object, + destination_location=destination_location, + ) + # AttemptGenerator owns resets and evaluates the saved term after final settling. Leaving + # terminations active would auto-reset the environment and destroy final-state evidence. + env_cfg.terminations = None + env_cfg.env_name = request.environment_name + if getattr(env_cfg, "observations", None) is not None and getattr(env_cfg.observations, "policy", None) is not None: + env_cfg.observations.policy.concatenate_terms = False + + recorder_cfg = ActionStateRecorderManagerCfg() + recorder_cfg.dataset_export_dir_path = dataset_export_dir_path + recorder_cfg.dataset_filename = dataset_filename + recorder_cfg.dataset_export_mode = ( + DatasetExportMode.EXPORT_SUCCEEDED_FAILED_IN_SEPARATE_FILES + if request.output.keep_failed + else DatasetExportMode.EXPORT_SUCCEEDED_ONLY + ) + env_cfg.recorders = recorder_cfg + + env = builder.make_registered(env_cfg=env_cfg, env_kwargs=env_kwargs) + try: + base_env = getattr(env, "unwrapped", env) + step_dt_s = float(base_env.step_dt) + validate_planner_timing(request.planner.interpolation_dt_s, step_dt_s) + adapter = make_embodiment_adapter(request) + adapter.bind_env(base_env) + except Exception: + env.close() + raise + return ArenaRuntimeBundle( + env=env, + embodiment_adapter=adapter, + success_term=success_term, + graph_spec=graph_spec, + step_dt_s=step_dt_s, + success_contract_evidence=success_contract_evidence, + runtime_asset_evidence=runtime_asset_evidence, + ) diff --git a/isaac_autodata_interfaces/autonomous/errors.py b/isaac_autodata_interfaces/autonomous/errors.py new file mode 100644 index 0000000..0ef26bd --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/errors.py @@ -0,0 +1,70 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Structured validation errors for autonomous request compilation.""" + +from __future__ import annotations + +import json +import re +from dataclasses import dataclass + +_SIMPLE_FIELD_RE = re.compile(r"^[A-Za-z_][A-Za-z0-9_]*$") + + +@dataclass(frozen=True) +class ValidationIssue: + """One request or catalog validation failure. + + Args: + path: Field path components. String components are mapping keys and integer components are + sequence indices. + code: Stable machine-readable failure code. + message: Safe human-readable explanation. + """ + + path: tuple[str | int, ...] + code: str + message: str + + @property + def field_path(self) -> str: + """Render :attr:`path` as an unambiguous JSONPath-like field path.""" + + rendered = "$" + for component in self.path: + if isinstance(component, int): + rendered += f"[{component}]" + elif _SIMPLE_FIELD_RE.fullmatch(component): + rendered += f".{component}" + else: + rendered += f"[{json.dumps(component, ensure_ascii=False)}]" + return rendered + + def to_dict(self) -> dict[str, str]: + """Return a JSON-serializable issue representation.""" + + return {"path": self.field_path, "code": self.code, "message": self.message} + + def __str__(self) -> str: + return f"{self.field_path} [{self.code}]: {self.message}" + + +class AutonomousValidationError(ValueError): + """Raised when semantic input or trusted catalog configuration is invalid. + + All discovered schema issues are available through :attr:`issues`. Fatal YAML construction + failures, such as duplicate keys, contain one precise issue. + """ + + def __init__(self, issues: list[ValidationIssue] | tuple[ValidationIssue, ...]) -> None: + assert issues, "AutonomousValidationError requires at least one issue" + self.issues = tuple(issues) + super().__init__("\n".join(str(issue) for issue in self.issues)) + + def to_dict(self) -> dict[str, list[dict[str, str]]]: + """Return all validation issues in a JSON-serializable envelope.""" + + return {"issues": [issue.to_dict() for issue in self.issues]} diff --git a/isaac_autodata_interfaces/autonomous/isaaclab_runtime.py b/isaac_autodata_interfaces/autonomous/isaaclab_runtime.py new file mode 100644 index 0000000..eb0cd51 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/isaaclab_runtime.py @@ -0,0 +1,1965 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Isaac Lab runtime and typed-plan executor for autonomous generation. + +This module intentionally uses duck-typed Isaac Lab objects. It can be imported and unit-tested +without launching SimulationApp; the live environment is required only when instances are used. +""" + +from __future__ import annotations + +import math +import time +import torch +from collections.abc import Callable, Mapping, Sequence +from dataclasses import dataclass, field +from typing import Any + +from isaac_autodata_core.autonomous.attempt_generation import ( + AttemptGenerationError, + AttemptRequest, + ExecutionResult, + FailureStage, +) +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + BarrierSegment, + CartesianTrajectorySegment, + ConcurrentGroupSegment, + DetachIntentSegment, + ExecutionEvent, + ExecutionEventType, + ExecutionOutcome, + GripperCommandMode, + GripperCommandSegment, + JointTrajectorySegment, + RobotStateSnapshot, + SceneObjectSnapshot, + SceneSnapshot, + TaskMotionPlan, + WaitSegment, + make_stable_id, +) +from isaac_autodata_interfaces.autonomous.pick_place_success import PickPlaceSuccessTracker + +SuccessVerifier = Callable[[Any, int], bool | Sequence[bool] | torch.Tensor] +AttachmentVerifier = Callable[[Any, Any, int, str, str], bool] + +# The current reviewed live executor is the normalized delta-pose Franka IK profile. Keep these +# limits local to that physical execution boundary until embodiment-specific limits become part of +# the public request contract. At 20 ms, the velocity limits admit at most 40 mm / 80 mrad between +# samples, well above the normal dense cuRobo interpolation while rejecting sparse teleports. +_RAW_DIK_POSE_ACTION_LIMIT = 1.0 +_RAW_DIK_SATURATION_TOLERANCE = 1e-6 +_MAX_CARTESIAN_TRANSLATION_STEP_M = 0.10 +_MAX_CARTESIAN_ROTATION_STEP_RAD = 0.25 +_MAX_CARTESIAN_LINEAR_VELOCITY_M_S = 2.0 +_MAX_CARTESIAN_ANGULAR_VELOCITY_RAD_S = 4.0 +_MAX_FRANKA_EEF_REACH_M = 1.0 +_RELEASE_CONTACT_POSITION_TOLERANCE_M = 0.020 +_RELEASE_CONTACT_ROTATION_TOLERANCE_RAD = 0.050 +_PHYSICAL_BOUNDARY_ABS_TOLERANCE = 1e-7 +_RETRYABLE_TRACKING_FAILURE_CODES = frozenset({ + "eef_position_tracking_error", + "eef_rotation_tracking_error", + "joint_path_tracking_error", +}) + + +@dataclass(frozen=True) +class _CartesianTargetState: + """Last collision-checked Cartesian target and optional joint-path diagnostics.""" + + pose: torch.Tensor + segment_id: str + sample_index: int + joint_seed_names: tuple[str, ...] + joint_seed: tuple[float, ...] | None + + +@dataclass +class AttachmentState: + """Executor-observed object held by each end effector.""" + + held_by_eef: dict[str, str | None] = field(default_factory=dict) + + def reset(self, eef_names: Sequence[str]) -> None: + """Clear all attachment state for a new episode.""" + + self.held_by_eef = {name: None for name in eef_names} + + +class IsaacLabAttemptRuntime: + """Reset, snapshot, and recorder lifecycle over one Isaac Lab environment.""" + + def __init__( + self, + env: Any, + embodiment_adapter: Any, + *, + graph_nodes: Sequence[Mapping[str, Any]], + attachment_state: AttachmentState | None = None, + reset_settle_steps: int = 10, + ) -> None: + if isinstance(reset_settle_steps, bool) or not isinstance(reset_settle_steps, int): + raise ValueError("reset_settle_steps must be an integer") + if not 0 <= reset_settle_steps <= 100: + raise ValueError("reset_settle_steps must be in [0, 100]") + self.env = _base_env(env) + self.embodiment_adapter = embodiment_adapter + self.graph_nodes = tuple(dict(node) for node in graph_nodes) + self.attachment_state = attachment_state or AttachmentState() + self.reset_settle_steps = reset_settle_steps + if getattr(embodiment_adapter, "env", None) is None: + embodiment_adapter.bind_env(self.env) + self._validate_live_scene_bindings() + self.attachment_state.reset(embodiment_adapter.get_eef_names()) + self._finalization_by_env: dict[int, tuple[str, bool, bool]] = {} + + async def reset_attempt(self, env_id: int) -> Mapping[str, Any]: + """Clear recorder/attachment buffers and reset one environment instance.""" + + # Beginning a new attempt invalidates the prior attempt's completion marker before any + # fallible reset operation. If recorder or environment reset fails, the orchestrator still + # owns a fresh attempt that must be finalized exactly once rather than being mistaken for + # the preceding completed attempt. + self._finalization_by_env.pop(env_id, None) + env_ids = _env_id_tensor(self.env, env_id) + recorder = getattr(self.env, "recorder_manager", None) + if recorder is not None: + recorder.reset(env_ids=env_ids) + self.attachment_state.reset(self.embodiment_adapter.get_eef_names()) + self.env.reset(env_ids=env_ids) + self._settle_reset_state(env_id) + # The reset transient and its open/hold actions are setup, not generated task data. + recorder_initial_state_recaptured = False + if recorder is not None and self.reset_settle_steps: + recorder.reset(env_ids=env_ids) + # Recorder reset removes the setup samples and the initial state captured by + # ``env.reset``. Re-run the public post-reset lifecycle at the settled state so the + # surviving episode starts with exactly one matching ``initial_state``. + recorder.record_post_reset(env_ids) + recorder_initial_state_recaptured = True + return { + "env_id": env_id, + "recorder_excludes_reset_settling": recorder is not None, + "recorder_initial_state_recaptured_after_settling": recorder_initial_state_recaptured, + "reset_completed": True, + "reset_settle_duration_s": self.reset_settle_steps * _step_dt(self.env), + "reset_settle_steps": self.reset_settle_steps, + } + + def _settle_reset_state(self, env_id: int) -> None: + """Hold the reset EEF pose with an open gripper while randomized objects settle.""" + + if not self.reset_settle_steps: + return + eef_names = tuple(self.embodiment_adapter.get_eef_names()) + if not eef_names: + raise ValueError("reset settling requires at least one end effector") + observed = self.embodiment_adapter.get_eef_poses(env_ids=[env_id]) + if set(observed) != set(eef_names): + raise ValueError("reset settling requires one observed pose for every end effector") + targets: dict[str, torch.Tensor] = {} + for name in eef_names: + pose = torch.as_tensor(observed[name][0], dtype=torch.float32, device=_device(self.env)) + if pose.shape != (4, 4) or not bool(torch.isfinite(pose).all().item()): + raise ValueError(f"reset settling observed an invalid pose for end effector {name!r}") + targets[name] = pose.detach().clone() + grippers = { + name: torch.ones( + int(self.embodiment_adapter.gripper_action_dim), + dtype=torch.float32, + device=_device(self.env), + ) + for name in eef_names + } + for _ in range(self.reset_settle_steps): + action = self.embodiment_adapter.target_eef_pose_to_action( + target_eef_pose_dict=targets, + gripper_action_dict=grippers, + action_noise_dict=None, + env_id=env_id, + ) + _validate_raw_dik_action(self.env, self.embodiment_adapter, action, context="reset settling") + action = torch.as_tensor(action) + batch = torch.zeros(self.env.action_space.shape, dtype=action.dtype, device=_device(self.env)) + batch[env_id] = action + self.env.step(batch) + + def capture_scene_snapshot(self, env_id: int, *, snapshot_id: str) -> SceneSnapshot: + """Capture finite robot/object state in the environment-origin frame.""" + + self._validate_live_scene_bindings() + joint_positions = _row_to_tuple(self.embodiment_adapter.get_joint_positions(env_ids=[env_id])[0]) + joint_names = tuple(self.embodiment_adapter.get_joint_names()) + eef_poses = { + name: _matrix_to_tuple(pose[0]) + for name, pose in self.embodiment_adapter.get_eef_poses(env_ids=[env_id]).items() + } + embodiment_node = next((node for node in self.graph_nodes if node.get("type") == "embodiment"), None) + robot_id = ( + str(embodiment_node["id"]) + if embodiment_node is not None + else str(getattr(self.embodiment_adapter, "name", "robot")) + ) + robot = RobotStateSnapshot( + robot_id=robot_id, + joint_names=joint_names, + joint_positions=joint_positions, + eef_poses=eef_poses, + held_objects=dict(self.attachment_state.held_by_eef), + ) + + graph_nodes = {str(node.get("id")): node for node in self.graph_nodes if node.get("id") is not None} + origin = _env_origin(self.env, env_id) + objects: list[SceneObjectSnapshot] = [] + for scene_id, scene_object in sorted(self.env.scene.rigid_objects.items()): + position = _tensor_row(scene_object.data.root_pos_w, env_id) - origin + quaternion_xyzw = _tensor_row(scene_object.data.root_quat_w, env_id) + pose = _pose_from_xyzw(position, quaternion_xyzw) + node = graph_nodes.get(scene_id) + roles = () if node is None else (str(node.get("type", "object")),) + geometry_ref = _geometry_ref(scene_object) + objects.append( + SceneObjectSnapshot( + semantic_id=scene_id, + scene_id=scene_id, + pose=_matrix_to_tuple(pose), + geometry_ref=geometry_ref, + roles=roles, + ) + ) + + return SceneSnapshot( + snapshot_id=snapshot_id, + captured_at_s=time.time(), + env_id=env_id, + robot=robot, + objects=tuple(objects), + metadata={ + "frame": "env_origin", + "rigid_object_quaternion_convention": "xyzw", + "step_dt_s": _step_dt(self.env), + "scene_binding": "validated_exact_v1", + }, + ) + + def _validate_live_scene_bindings(self) -> None: + """Require an exact semantic-to-live binding before planning can observe the scene.""" + + node_ids = [node.get("id") for node in self.graph_nodes] + if any(not isinstance(node_id, str) or not node_id for node_id in node_ids): + raise ValueError("every linked graph node must have a non-empty string id") + if len(node_ids) != len(set(node_ids)): + raise ValueError("linked graph node ids must be unique") + embodiment_nodes = [node for node in self.graph_nodes if node.get("type") == "embodiment"] + if len(embodiment_nodes) != 1: + raise ValueError(f"live scene binding requires exactly one embodiment node, got {len(embodiment_nodes)}") + articulations = getattr(self.env.scene, "articulations", None) + if not isinstance(articulations, Mapping) or len(articulations) != 1: + names = () if not isinstance(articulations, Mapping) else tuple(articulations) + raise ValueError(f"live scene binding requires exactly one articulation, got {names}") + semantic_objects = { + str(node["id"]) for node in self.graph_nodes if node.get("type") in ("object", "object_reference") + } + rigid_objects = getattr(self.env.scene, "rigid_objects", None) + if not isinstance(rigid_objects, Mapping): + raise ValueError("live scene does not expose a rigid-object mapping") + live_objects = set(rigid_objects) + if live_objects != semantic_objects: + missing = sorted(semantic_objects - live_objects) + unexpected = sorted(live_objects - semantic_objects) + raise ValueError( + "linked object ids must bind exactly to live rigid objects " + f"(missing={missing}, unexpected={unexpected})" + ) + for name, scene_object in rigid_objects.items(): + geometry_ref = _geometry_ref(scene_object) + if not isinstance(geometry_ref, str) or not geometry_ref: + raise ValueError(f"live rigid object {name!r} has no stable geometry reference") + + async def finish_attempt(self, env_id: int, *, success: bool, keep_failed: bool) -> None: + """Stamp final success and export the recorder buffer according to retention policy.""" + + requested = (success, keep_failed) + prior = self._finalization_by_env.get(env_id) + if prior is not None: + state, prior_success, prior_keep_failed = prior + if (prior_success, prior_keep_failed) != requested: + raise RuntimeError(f"attempt for env {env_id} was already finalized with different retention state") + if state == "complete": + return + raise RuntimeError(f"attempt finalization for env {env_id} is {state}; refusing a duplicate export") + self._finalization_by_env[env_id] = ("in_progress", success, keep_failed) + recorder = getattr(self.env, "recorder_manager", None) + if recorder is None: + self._finalization_by_env[env_id] = ("complete", success, keep_failed) + return + try: + env_ids = _env_id_tensor(self.env, env_id) + success_tensor = torch.tensor([[success]], dtype=torch.bool, device=_device(self.env)) + recorder.set_success_to_episodes(env_ids, success_tensor) + if success or keep_failed: + recorder.export_episodes(env_ids) + except BaseException: + self._finalization_by_env[env_id] = ("failed", success, keep_failed) + raise + self._finalization_by_env[env_id] = ("complete", success, keep_failed) + + +class IsaacLabPlanExecutor: + """Execute a validated, currently single-arm task-motion plan in Isaac Lab.""" + + def __init__( + self, + env: Any, + embodiment_adapter: Any, + success_verifier: SuccessVerifier, + *, + attachment_state: AttachmentState | None = None, + attachment_verifier: AttachmentVerifier | None = None, + final_settle_steps: int = 5, + final_stability_steps: int | None = None, + max_steps: int = 20_000, + attachment_distance_m: float = 0.05, + max_eef_position_error_m: float = 0.08, + max_eef_rotation_error_rad: float = 0.50, + max_joint_path_error_rad: float = 0.35, + max_tracking_correction_steps: int = 12, + interaction_position_tolerance_m: float = 0.005, + interaction_rotation_tolerance_rad: float = 0.05, + max_interaction_correction_steps: int = 12, + max_terminal_correction_steps: int = 32, + ) -> None: + if final_settle_steps < 0: + raise ValueError("final_settle_steps must be non-negative") + if final_stability_steps is None: + final_stability_steps = min(5, final_settle_steps + 1) + if final_stability_steps < 1 or final_stability_steps > final_settle_steps + 1: + raise ValueError("final_stability_steps must be in [1, final_settle_steps + 1]") + if max_steps <= 0: + raise ValueError("max_steps must be positive") + if ( + isinstance(max_tracking_correction_steps, bool) + or not isinstance(max_tracking_correction_steps, int) + or not 0 <= max_tracking_correction_steps <= 100 + ): + raise ValueError("max_tracking_correction_steps must be an integer in [0, 100]") + if ( + isinstance(max_interaction_correction_steps, bool) + or not isinstance(max_interaction_correction_steps, int) + or not 0 <= max_interaction_correction_steps <= 100 + ): + raise ValueError("max_interaction_correction_steps must be an integer in [0, 100]") + if ( + isinstance(max_terminal_correction_steps, bool) + or not isinstance(max_terminal_correction_steps, int) + or not 0 <= max_terminal_correction_steps <= 100 + ): + raise ValueError("max_terminal_correction_steps must be an integer in [0, 100]") + if not math.isfinite(attachment_distance_m) or attachment_distance_m <= 0: + raise ValueError("attachment_distance_m must be finite and positive") + for field_name, value in ( + ("max_eef_position_error_m", max_eef_position_error_m), + ("max_eef_rotation_error_rad", max_eef_rotation_error_rad), + ("interaction_position_tolerance_m", interaction_position_tolerance_m), + ("interaction_rotation_tolerance_rad", interaction_rotation_tolerance_rad), + ): + if not math.isfinite(value) or value <= 0: + raise ValueError(f"{field_name} must be finite and positive") + if ( + isinstance(max_joint_path_error_rad, bool) + or not isinstance(max_joint_path_error_rad, (int, float)) + or not math.isfinite(max_joint_path_error_rad) + or max_joint_path_error_rad <= 0 + ): + raise ValueError("max_joint_path_error_rad must be finite and positive") + if interaction_position_tolerance_m > max_eef_position_error_m: + raise ValueError("interaction_position_tolerance_m must not exceed max_eef_position_error_m") + if interaction_rotation_tolerance_rad > max_eef_rotation_error_rad: + raise ValueError("interaction_rotation_tolerance_rad must not exceed max_eef_rotation_error_rad") + self.env = _base_env(env) + self.embodiment_adapter = embodiment_adapter + self.success_verifier = success_verifier + self.attachment_state = attachment_state or AttachmentState() + self.attachment_verifier = attachment_verifier + self.final_settle_steps = final_settle_steps + self.final_stability_steps = final_stability_steps + self.max_steps = max_steps + self.attachment_distance_m = attachment_distance_m + self.max_eef_position_error_m = max_eef_position_error_m + self.max_eef_rotation_error_rad = max_eef_rotation_error_rad + # ScheduleStream's native DIK PathController can carry cuRobo joint samples as diagnostics + # while commanding only Cartesian poses. Enforce a finite null-space corridor whenever + # those diagnostics are present; plans that legitimately omit them remain Cartesian-only. + self.max_joint_path_error_rad = max_joint_path_error_rad + self.max_tracking_correction_steps = max_tracking_correction_steps + self.interaction_position_tolerance_m = interaction_position_tolerance_m + self.interaction_rotation_tolerance_rad = interaction_rotation_tolerance_rad + self.max_interaction_correction_steps = max_interaction_correction_steps + self.max_terminal_correction_steps = max_terminal_correction_steps + self._gripper_values: dict[str, float] = {} + self._last_cartesian_targets: dict[str, _CartesianTargetState] = {} + self._step_count = 0 + self._tracking_correction_steps = 0 + self._maximum_eef_position_error_m = 0.0 + self._maximum_eef_rotation_error_rad = 0.0 + self._maximum_joint_path_error_rad = 0.0 + self._maximum_joint_path_error_name: str | None = None + self._gripper_milestones: list[dict[str, Any]] = [] + self._interaction_boundaries: list[dict[str, Any]] = [] + self._interaction_correction_steps = 0 + self._terminal_target_boundaries: list[dict[str, Any]] = [] + self._terminal_correction_steps = 0 + self._event_count = 0 + self._pick_place_success: PickPlaceSuccessTracker | None = None + + async def execute(self, request: AttemptRequest, plan: TaskMotionPlan) -> ExecutionResult: + """Execute segments in declared order and verify only the settled final state.""" + + events: list[ExecutionEvent] = [] + self._step_count = 0 + self._tracking_correction_steps = 0 + self._maximum_eef_position_error_m = 0.0 + self._maximum_eef_rotation_error_rad = 0.0 + self._maximum_joint_path_error_rad = 0.0 + self._maximum_joint_path_error_name = None + self._gripper_milestones = [] + self._interaction_boundaries = [] + self._interaction_correction_steps = 0 + self._terminal_target_boundaries = [] + self._terminal_correction_steps = 0 + self._last_cartesian_targets = {} + self._event_count = 0 + self._pick_place_success = None + eef_names = tuple(self.embodiment_adapter.get_eef_names()) + if not eef_names: + raise AttemptGenerationError(FailureStage.EXECUTION, "no_end_effector", "adapter has no end effector") + self._gripper_values = {name: 1.0 for name in eef_names} + if not self.attachment_state.held_by_eef: + self.attachment_state.reset(eef_names) + + completed: set[str] = set() + try: + try: + self._pick_place_success = PickPlaceSuccessTracker.from_plan(plan, eef_names) + except ValueError as exc: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "pick_place_lifecycle_invalid", + str(exc), + recoverable=False, + ) from exc + # Validate the complete plan before taking the first action. In particular, a native + # ScheduleStream stream may contain an otherwise valid Cartesian prefix followed by a + # joint or concurrent segment that this IK executor cannot represent. Discovering that + # only after executing the prefix would leave the robot in an unaccounted partial state. + self._validate_plan(plan, eef_names, request.env_id) + events.append(self._event(request, plan, None, ExecutionEventType.PLAN_STARTED, ExecutionOutcome.PENDING)) + for segment in plan.segments: + missing = set(segment.depends_on) - completed + if missing: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "dependency_not_completed", + f"segment {segment.segment_id!r} dependencies were not completed: {sorted(missing)}", + recoverable=False, + ) + events.append( + self._event( + request, + plan, + segment.segment_id, + ExecutionEventType.SEGMENT_STARTED, + ExecutionOutcome.PENDING, + ) + ) + self._execute_segment(request.env_id, segment) + completed.add(segment.segment_id) + events.append( + self._event( + request, + plan, + segment.segment_id, + ExecutionEventType.SEGMENT_COMPLETED, + ExecutionOutcome.SUCCEEDED, + ) + ) + + self._converge_terminal_targets(request.env_id, eef_names) + success_streak = 0 + for verification_samples, settle_index in enumerate(range(self.final_settle_steps + 1), start=1): + if settle_index: + self._hold(request.env_id, 1) + goal_satisfied = _env_bool(self.success_verifier(self.env, request.env_id), request.env_id) + if self._pick_place_success is not None: + self._observe_task_success_final(request.env_id, goal_satisfied=goal_satisfied) + if goal_satisfied: + success_streak += 1 + else: + success_streak = 0 + self._verify_terminal_targets_after_settling( + request.env_id, + eef_names, + verification_samples=verification_samples, + ) + arena_success = success_streak >= self.final_stability_steps + physical_observation = self._physical_observation(request.env_id) + task_success_report = physical_observation.get("task_success_report") + task_success = task_success_report is None or task_success_report["passed"] is True + if not arena_success or not task_success: + if arena_success: + failure_code = "task_success_checks_failed" + failure_message = "live task-success checks rejected the physical pick-and-place result" + else: + failure_code = "final_goal_not_satisfied" + failure_message = "task success predicate was not stable after final settling" + events.append( + self._event( + request, + plan, + None, + ExecutionEventType.TASK_REJECTED, + ExecutionOutcome.FAILED, + failure_code=failure_code, + message=failure_message, + ) + ) + return ExecutionResult( + success=False, + events=tuple(events), + final_observation={ + "final_success": False, + "final_success_streak": success_streak, + "steps": self._step_count, + "tracking_correction_steps": self._tracking_correction_steps, + **self._tracking_observation(), + **physical_observation, + "verification_samples": verification_samples, + }, + failure_stage=FailureStage.VERIFICATION, + failure_code=failure_code, + failure_message=failure_message, + ) + events.append( + self._event(request, plan, None, ExecutionEventType.TASK_VERIFIED, ExecutionOutcome.SUCCEEDED) + ) + events.append( + self._event(request, plan, None, ExecutionEventType.PLAN_COMPLETED, ExecutionOutcome.SUCCEEDED) + ) + return ExecutionResult( + success=True, + events=tuple(events), + final_observation={ + "final_success": True, + "final_success_streak": success_streak, + "steps": self._step_count, + "tracking_correction_steps": self._tracking_correction_steps, + **self._tracking_observation(), + **physical_observation, + "verification_samples": verification_samples, + }, + ) + except Exception as exc: + error = ( + exc + if isinstance(exc, AttemptGenerationError) + else AttemptGenerationError(FailureStage.EXECUTION, "segment_execution_failed", str(exc)) + ) + events.append( + self._event( + request, + plan, + None, + ExecutionEventType.PLAN_FAILED, + ExecutionOutcome.FAILED, + failure_code=error.code, + message=str(error)[:2048], + ) + ) + return ExecutionResult( + success=False, + events=tuple(events), + final_observation={ + "final_success": False, + "steps": self._step_count, + "tracking_correction_steps": self._tracking_correction_steps, + **self._tracking_observation(), + **self._physical_observation(request.env_id), + }, + failure_stage=error.stage, + failure_code=error.code, + failure_message=str(error), + recoverable=error.recoverable, + ) + + def _validate_plan(self, plan: TaskMotionPlan, eef_names: tuple[str, ...], env_id: int) -> None: + """Fail before execution when a segment or total step budget is unsupported.""" + + estimated_steps = self.final_settle_steps + known_eefs = set(eef_names) + known_joints = set(self.embodiment_adapter.get_joint_names()) + predicted_poses: dict[str, torch.Tensor] | None = None + robot_base_position: torch.Tensor | None = None + predicted_gripper_values = {eef_name: 1.0 for eef_name in eef_names} + eefs_with_cartesian_target: set[str] = set() + prior_segment_ids: set[str] = set() + for segment in plan.segments: + missing_prior_dependencies = set(segment.depends_on) - prior_segment_ids + if missing_prior_dependencies: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "plan_not_topologically_ordered", + f"segment {segment.segment_id!r} depends on later segments {sorted(missing_prior_dependencies)}", + recoverable=False, + ) + if isinstance(segment, JointTrajectorySegment): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "joint_trajectory_not_supported_by_ik_executor", + "joint trajectories require a joint-control embodiment executor", + recoverable=False, + ) + if isinstance(segment, ConcurrentGroupSegment): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "concurrent_group_not_supported_by_single_arm_executor", + "concurrent groups require a multi-arm executor", + recoverable=False, + ) + if not isinstance( + segment, + ( + CartesianTrajectorySegment, + GripperCommandSegment, + AttachIntentSegment, + DetachIntentSegment, + WaitSegment, + BarrierSegment, + ), + ): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unsupported_segment", + f"unsupported plan segment {type(segment).__name__}", + recoverable=False, + ) + eef_name = getattr(segment, "eef_name", None) + if eef_name is not None and eef_name not in known_eefs: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unknown_end_effector", + f"plan references unknown end effector {eef_name!r}", + recoverable=False, + ) + if isinstance(segment, CartesianTrajectorySegment): + if segment.frame not in ("env", "env_origin", "world"): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unsupported_pose_frame", + f"executor supports only 'env_origin' and 'world' Cartesian poses, got {segment.frame!r}", + recoverable=False, + ) + missing_joints = set(segment.joint_seed_names) - known_joints + if missing_joints: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "joint_tracking_observation_missing", + f"joint tracking observations are missing {sorted(missing_joints)}", + recoverable=False, + ) + if predicted_poses is None: + predicted_poses = self._initial_eef_poses(env_id, eef_names) + robot_base_position = _robot_base_position_in_env(self.env, env_id) + self._validate_cartesian_trajectory( + env_id, + segment, + predicted_poses, + robot_base_position, + ) + eefs_with_cartesian_target.add(segment.eef_name) + estimated_steps += len(segment.poses) * (1 + self.max_tracking_correction_steps) + elif isinstance(segment, GripperCommandSegment): + requested_gripper_value = _gripper_value(segment) + previous_gripper_value = predicted_gripper_values[segment.eef_name] + if requested_gripper_value != previous_gripper_value: + if segment.eef_name not in eefs_with_cartesian_target: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "interaction_target_missing", + f"gripper transition {segment.segment_id!r} has no preceding collision-checked " + f"Cartesian target for {segment.eef_name!r}", + recoverable=False, + ) + estimated_steps += self.max_interaction_correction_steps + predicted_gripper_values[segment.eef_name] = requested_gripper_value + estimated_steps += segment.settle_steps + elif isinstance(segment, WaitSegment): + estimated_steps += ( + segment.steps + if segment.steps is not None + else max(1, math.ceil((segment.duration_s or 0.0) / _step_dt(self.env))) + ) + prior_segment_ids.add(segment.segment_id) + # The ordinary path corridor bounds transient DIK error but is intentionally wider than + # the terminal physical-evidence boundary. Reserve one strict, bounded convergence window + # for every end effector that received a collision-checked Cartesian target. + estimated_steps += len(eefs_with_cartesian_target) * self.max_terminal_correction_steps + if estimated_steps > self.max_steps: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "execution_step_limit", + f"plan requires approximately {estimated_steps} simulator steps; limit is {self.max_steps}", + ) + + def _initial_eef_poses(self, env_id: int, eef_names: tuple[str, ...]) -> dict[str, torch.Tensor]: + """Read finite live EEF poses used as the first trajectory continuity anchors.""" + + try: + observed = self.embodiment_adapter.get_eef_poses(env_ids=[env_id]) + except Exception as exc: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "tracking_observation_missing", + f"failed to read initial end-effector poses: {type(exc).__name__}: {str(exc)[:512]}", + recoverable=False, + ) from exc + poses: dict[str, torch.Tensor] = {} + for eef_name in eef_names: + if eef_name not in observed: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "tracking_observation_missing", + f"no initial observed pose is available for end effector {eef_name!r}", + recoverable=False, + ) + pose = torch.as_tensor(observed[eef_name][0], dtype=torch.float64, device=_device(self.env)) + if pose.shape != (4, 4) or not bool(torch.isfinite(pose).all().item()): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "tracking_observation_invalid", + f"initial observed pose for {eef_name!r} must be a finite 4x4 transform", + recoverable=False, + ) + poses[eef_name] = pose + return poses + + def _validate_cartesian_trajectory( + self, + env_id: int, + segment: CartesianTrajectorySegment, + predicted_poses: dict[str, torch.Tensor], + robot_base_position: torch.Tensor, + ) -> None: + """Validate every Cartesian sample against live Franka execution limits.""" + + step_dt_s = _cartesian_step_dt(segment, self.env) + previous = predicted_poses[segment.eef_name] + for sample_index, pose in enumerate(segment.poses): + target = torch.tensor(pose, dtype=torch.float64, device=_device(self.env)) + target = _target_in_env_frame(self.env, env_id, target, segment.frame) + distance_from_base = float(torch.linalg.norm(target[:3, 3] - robot_base_position).item()) + if not math.isfinite(distance_from_base) or distance_from_base > _MAX_FRANKA_EEF_REACH_M: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "eef_workspace_limit", + f"segment {segment.segment_id!r} sample {sample_index} is {distance_from_base:.6g}m " + f"from the robot base; Franka execution limit is {_MAX_FRANKA_EEF_REACH_M:.6g}m", + ) + + translation_step = float(torch.linalg.norm(target[:3, 3] - previous[:3, 3]).item()) + rotation_step = _rotation_distance_rad(previous[:3, :3], target[:3, :3]) + if translation_step > _MAX_CARTESIAN_TRANSLATION_STEP_M + 1e-9: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_translation_discontinuity", + f"segment {segment.segment_id!r} sample {sample_index} translates {translation_step:.6g}m; " + f"limit is {_MAX_CARTESIAN_TRANSLATION_STEP_M:.6g}m per sample", + ) + if rotation_step > _MAX_CARTESIAN_ROTATION_STEP_RAD + 1e-9: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_rotation_discontinuity", + f"segment {segment.segment_id!r} sample {sample_index} rotates {rotation_step:.6g}rad; " + f"limit is {_MAX_CARTESIAN_ROTATION_STEP_RAD:.6g}rad per sample", + ) + linear_velocity = translation_step / step_dt_s + angular_velocity = rotation_step / step_dt_s + if linear_velocity > _MAX_CARTESIAN_LINEAR_VELOCITY_M_S + 1e-6: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_linear_velocity_limit", + f"segment {segment.segment_id!r} sample {sample_index} implies {linear_velocity:.6g}m/s; " + f"limit is {_MAX_CARTESIAN_LINEAR_VELOCITY_M_S:.6g}m/s", + ) + if angular_velocity > _MAX_CARTESIAN_ANGULAR_VELOCITY_RAD_S + 1e-6: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_angular_velocity_limit", + f"segment {segment.segment_id!r} sample {sample_index} implies {angular_velocity:.6g}rad/s; " + f"limit is {_MAX_CARTESIAN_ANGULAR_VELOCITY_RAD_S:.6g}rad/s", + ) + previous = target + predicted_poses[segment.eef_name] = previous + + def _execute_segment(self, env_id: int, segment: Any) -> None: + if isinstance(segment, CartesianTrajectorySegment): + self._require_eef(segment.eef_name) + for sample_index, pose in enumerate(segment.poses): + target = torch.tensor(pose, dtype=torch.float32, device=_device(self.env)) + target = _target_in_env_frame(self.env, env_id, target, segment.frame) + joint_seed = None if not segment.joint_seeds else segment.joint_seeds[sample_index] + self._step_target( + env_id, + segment.eef_name, + target, + joint_seed_names=segment.joint_seed_names, + joint_seed=joint_seed, + ) + self._last_cartesian_targets[segment.eef_name] = _CartesianTargetState( + pose=target.detach().clone(), + segment_id=segment.segment_id, + sample_index=sample_index, + joint_seed_names=tuple(segment.joint_seed_names), + joint_seed=None if joint_seed is None else tuple(float(value) for value in joint_seed), + ) + return + if isinstance(segment, GripperCommandSegment): + self._require_eef(segment.eef_name) + requested_gripper_value = _gripper_value(segment) + interaction_evidence = self._prepare_gripper_interaction( + env_id, + segment, + requested_gripper_value, + ) + if ( + self._pick_place_success is not None + and segment.segment_id == self._pick_place_success.release_open_segment_id + ): + self._update_pick_place_success( + "begin_release", + segment.segment_id, + self._task_success_sample(env_id), + ) + self._gripper_values[segment.eef_name] = requested_gripper_value + self._hold(env_id, segment.settle_steps) + milestone = self._capture_physical_state(env_id) + milestone.update({ + "command": segment.command.value, + "interaction_boundary": dict(interaction_evidence), + "segment_id": segment.segment_id, + "step": self._step_count, + }) + self._gripper_milestones.append(milestone) + if ( + self._pick_place_success is not None + and segment.segment_id == self._pick_place_success.initial_open_segment_id + ): + initial_goal_satisfied = _env_bool(self.success_verifier(self.env, env_id), env_id) + self._update_pick_place_success( + "settle_initial", + segment.segment_id, + self._task_success_sample(env_id), + goal_satisfied=initial_goal_satisfied, + ) + return + if isinstance(segment, AttachIntentSegment): + self._require_eef(segment.eef_name) + if self.attachment_state.held_by_eef.get(segment.eef_name) is not None: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "end_effector_already_holding", + f"{segment.eef_name!r} already holds an object", + ) + verified = ( + self.attachment_verifier( + self.env, + self.embodiment_adapter, + env_id, + segment.eef_name, + segment.object_name, + ) + if self.attachment_verifier is not None + else self._verify_attachment_proximity(env_id, segment.eef_name, segment.object_name) + ) + if not verified: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "grasp_not_verified", + f"attachment verifier {segment.verifier!r} rejected {segment.object_name!r}", + ) + task_success_sample = None + if self._pick_place_success is not None: + task_success_sample = self._task_success_sample(env_id) + physically_plausible = self._update_pick_place_success( + "attachment_is_plausible", + task_success_sample, + ) + if physically_plausible is not True: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "grasp_not_verified", + f"physical aperture/distance evidence rejected {segment.object_name!r}", + ) + self._update_pick_place_success( + "attach", + segment.segment_id, + segment.object_name, + segment.eef_name, + task_success_sample, + ) + self.attachment_state.held_by_eef[segment.eef_name] = segment.object_name + return + if isinstance(segment, DetachIntentSegment): + self._require_eef(segment.eef_name) + held = self.attachment_state.held_by_eef.get(segment.eef_name) + if held != segment.object_name: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "detach_object_mismatch", + f"cannot detach {segment.object_name!r}; {segment.eef_name!r} holds {held!r}", + ) + if self._pick_place_success is not None: + self._update_pick_place_success( + "detach", + segment.segment_id, + segment.object_name, + segment.eef_name, + self._task_success_sample(env_id), + ) + self.attachment_state.held_by_eef[segment.eef_name] = None + return + if isinstance(segment, WaitSegment): + steps = segment.steps + if steps is None: + steps = max(1, math.ceil((segment.duration_s or 0.0) / _step_dt(self.env))) + self._hold(env_id, steps) + return + if isinstance(segment, BarrierSegment): + return + if isinstance(segment, JointTrajectorySegment): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "joint_trajectory_not_supported_by_ik_executor", + "joint trajectories require a joint-control embodiment executor", + recoverable=False, + ) + if isinstance(segment, ConcurrentGroupSegment): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "concurrent_group_not_supported_by_single_arm_executor", + "concurrent groups require a multi-arm executor", + recoverable=False, + ) + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unsupported_segment", + f"unsupported plan segment {type(segment).__name__}", + recoverable=False, + ) + + def _prepare_gripper_interaction( + self, + env_id: int, + segment: GripperCommandSegment, + requested_gripper_value: float, + ) -> dict[str, Any]: + """Converge to the preceding checked target before changing the gripper command.""" + + previous_gripper_value = self._gripper_values[segment.eef_name] + target_state = self._last_cartesian_targets.get(segment.eef_name) + transition_required = requested_gripper_value != previous_gripper_value + task_success_release = ( + self._pick_place_success is not None + and segment.segment_id == self._pick_place_success.release_open_segment_id + ) + evidence: dict[str, Any] = { + "command": segment.command.value, + "contact_constrained_cartesian_ready": None, + "contact_constrained_task_success_accepted": False, + "convergence_mode": None, + "converged": None, + "correction_steps": 0, + "final_position_error_m": None, + "final_rotation_error_rad": None, + "initial_position_error_m": None, + "initial_rotation_error_rad": None, + "interaction_position_tolerance_m": self.interaction_position_tolerance_m, + "interaction_rotation_tolerance_rad": self.interaction_rotation_tolerance_rad, + "joint_diagnostics_present": target_state is not None and target_state.joint_seed is not None, + "max_correction_steps": self.max_interaction_correction_steps, + "outcome": "pending", + "preceding_cartesian_target": target_state is not None, + "previous_gripper_value": previous_gripper_value, + "release_contact_gate_required": task_success_release, + "release_contact_position_tolerance_m": ( + _RELEASE_CONTACT_POSITION_TOLERANCE_M if task_success_release else None + ), + "release_contact_rotation_tolerance_rad": ( + _RELEASE_CONTACT_ROTATION_TOLERANCE_RAD if task_success_release else None + ), + "requested_gripper_value": requested_gripper_value, + "task_success_release_gate": None, + "segment_id": segment.segment_id, + "strict_target_converged": None, + "target_sample_index": None if target_state is None else target_state.sample_index, + "target_segment_id": None if target_state is None else target_state.segment_id, + "transition_required": transition_required, + } + self._interaction_boundaries.append(evidence) + if not transition_required: + evidence["outcome"] = ( + "not_required_no_transition" if target_state is not None else "not_required_no_preceding_target" + ) + return evidence + if target_state is None: + evidence["converged"] = False + evidence["outcome"] = "failed_no_preceding_target" + raise AttemptGenerationError( + FailureStage.EXECUTION, + "interaction_target_missing", + f"gripper transition {segment.segment_id!r} has no preceding collision-checked Cartesian target", + recoverable=False, + ) + + position_error, rotation_error, path_verified, strict_target_converged = self._interaction_target_status( + env_id, + segment.eef_name, + target_state, + ) + evidence["initial_position_error_m"] = position_error + evidence["initial_rotation_error_rad"] = rotation_error + correction_steps = 0 + while not strict_target_converged and correction_steps < self.max_interaction_correction_steps: + step_count_before = self._step_count + try: + # The requested value is intentionally not installed until this loop succeeds. + self._step_targets( + env_id, + {segment.eef_name: target_state.pose}, + joint_seed_names=target_state.joint_seed_names, + joint_seed=target_state.joint_seed, + ) + except AttemptGenerationError as exc: + executed_steps = self._step_count - step_count_before + correction_steps += executed_steps + self._interaction_correction_steps += executed_steps + if exc.code not in _RETRYABLE_TRACKING_FAILURE_CODES: + evidence["converged"] = False + evidence["correction_steps"] = correction_steps + evidence["outcome"] = f"failed_{exc.code}" + raise + else: + executed_steps = self._step_count - step_count_before + correction_steps += executed_steps + self._interaction_correction_steps += executed_steps + position_error, rotation_error, path_verified, strict_target_converged = self._interaction_target_status( + env_id, + segment.eef_name, + target_state, + ) + + evidence["correction_steps"] = correction_steps + evidence["final_position_error_m"] = position_error + evidence["final_rotation_error_rad"] = rotation_error + evidence["strict_target_converged"] = strict_target_converged + contact_constrained_cartesian_ready = ( + task_success_release + and path_verified + and _finite_at_most(position_error, _RELEASE_CONTACT_POSITION_TOLERANCE_M) + and _finite_at_most(rotation_error, _RELEASE_CONTACT_ROTATION_TOLERANCE_RAD) + ) + evidence["contact_constrained_cartesian_ready"] = contact_constrained_cartesian_ready + + task_success_release_ready = False + if task_success_release: + try: + task_success_gate = self._update_pick_place_success( + "release_contact_gate", + segment.segment_id, + self._task_success_sample(env_id), + ) + except AttemptGenerationError: + evidence["converged"] = False + evidence["outcome"] = "failed_task_success_unavailable" + raise + evidence["task_success_release_gate"] = task_success_gate + task_success_release_ready = task_success_gate.get("passed") is True + if not task_success_release_ready: + evidence["converged"] = False + evidence["outcome"] = "failed_task_success_release_not_ready" + raise AttemptGenerationError( + FailureStage.EXECUTION, + "release_task_success_not_ready", + f"release transition {segment.segment_id!r} lacks validated pre-release placement/transport", + ) + + accepted = strict_target_converged and (not task_success_release or task_success_release_ready) + if task_success_release and not accepted and contact_constrained_cartesian_ready and task_success_release_ready: + accepted = True + evidence["contact_constrained_task_success_accepted"] = True + evidence["convergence_mode"] = "contact_constrained_task_success_ready" + elif accepted and task_success_release: + evidence["convergence_mode"] = "strict_task_success_ready" + elif accepted: + evidence["convergence_mode"] = "strict_target" + + evidence["converged"] = accepted + if not accepted: + evidence["outcome"] = "failed_tolerance_not_reached" + raise AttemptGenerationError( + FailureStage.EXECUTION, + "interaction_target_not_reached", + f"gripper transition {segment.segment_id!r} remained {position_error:.6g}m / " + f"{rotation_error:.6g}rad from its preceding Cartesian target after " + f"{correction_steps} bounded correction steps", + ) + evidence["outcome"] = evidence["convergence_mode"] if task_success_release else "converged" + return evidence + + def _converge_terminal_targets(self, env_id: int, eef_names: Sequence[str]) -> None: + """Converge final checked targets before settled task-success verification.""" + + for eef_name in eef_names: + target_state = self._last_cartesian_targets.get(eef_name) + if target_state is None: + continue + gripper_value = self._gripper_values[eef_name] + evidence: dict[str, Any] = { + "correction_steps": 0, + "eef_name": eef_name, + "final_joint_path_verified": None, + "final_position_error_m": None, + "final_rotation_error_rad": None, + "gripper_unchanged": None, + "gripper_value": gripper_value, + "initial_joint_path_verified": None, + "initial_position_error_m": None, + "initial_rotation_error_rad": None, + "interaction_position_tolerance_m": self.interaction_position_tolerance_m, + "interaction_rotation_tolerance_rad": self.interaction_rotation_tolerance_rad, + "joint_diagnostics_present": target_state.joint_seed is not None, + "max_correction_steps": self.max_terminal_correction_steps, + "outcome": "pending", + "target_sample_index": target_state.sample_index, + "target_segment_id": target_state.segment_id, + "convergence_trace": [], + } + self._terminal_target_boundaries.append(evidence) + position_error, rotation_error, path_verified, converged = self._interaction_target_status( + env_id, + eef_name, + target_state, + ) + evidence["initial_position_error_m"] = _finite_or_none(position_error) + evidence["initial_rotation_error_rad"] = _finite_or_none(rotation_error) + evidence["initial_joint_path_verified"] = path_verified + correction_steps = 0 + evidence["convergence_trace"].append({ + "correction_steps": correction_steps, + "joint_path_verified": path_verified, + "position_error_m": _finite_or_none(position_error), + "rotation_error_rad": _finite_or_none(rotation_error), + }) + while not converged and correction_steps < self.max_terminal_correction_steps: + step_count_before = self._step_count + try: + self._step_targets( + env_id, + {eef_name: target_state.pose}, + joint_seed_names=target_state.joint_seed_names, + joint_seed=target_state.joint_seed, + ) + except AttemptGenerationError as exc: + executed_steps = self._step_count - step_count_before + correction_steps += executed_steps + self._terminal_correction_steps += executed_steps + if exc.code not in _RETRYABLE_TRACKING_FAILURE_CODES: + evidence["correction_steps"] = correction_steps + evidence["outcome"] = f"failed_{exc.code}" + raise + else: + executed_steps = self._step_count - step_count_before + correction_steps += executed_steps + self._terminal_correction_steps += executed_steps + position_error, rotation_error, path_verified, converged = self._interaction_target_status( + env_id, + eef_name, + target_state, + ) + evidence["convergence_trace"].append({ + "correction_steps": correction_steps, + "joint_path_verified": path_verified, + "position_error_m": _finite_or_none(position_error), + "rotation_error_rad": _finite_or_none(rotation_error), + }) + + evidence["correction_steps"] = correction_steps + evidence["final_position_error_m"] = _finite_or_none(position_error) + evidence["final_rotation_error_rad"] = _finite_or_none(rotation_error) + evidence["final_joint_path_verified"] = path_verified + evidence["gripper_unchanged"] = self._gripper_values[eef_name] == gripper_value + assert len(evidence["convergence_trace"]) <= self.max_terminal_correction_steps + 1 + if not converged: + evidence["outcome"] = "failed_tolerance_not_reached" + raise AttemptGenerationError( + FailureStage.EXECUTION, + "terminal_target_not_reached", + f"terminal target for {eef_name!r} remained {position_error:.6g}m / " + f"{rotation_error:.6g}rad from segment {target_state.segment_id!r} after " + f"{correction_steps} bounded correction steps", + ) + evidence["outcome"] = "already_converged" if correction_steps == 0 else "converged" + + def _interaction_target_status( + self, + env_id: int, + eef_name: str, + target_state: _CartesianTargetState, + ) -> tuple[float, float, bool, bool]: + """Return strict interaction errors and combined Cartesian/joint convergence.""" + + observed_poses = self.embodiment_adapter.get_eef_poses(env_ids=[env_id]) + if eef_name not in observed_poses: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "tracking_observation_missing", + f"no observed pose is available for end effector {eef_name!r}", + recoverable=False, + ) + observed = torch.as_tensor( + observed_poses[eef_name][0], + device=target_state.pose.device, + dtype=target_state.pose.dtype, + ) + position_error = float(torch.linalg.norm(observed[:3, 3] - target_state.pose[:3, 3]).item()) + rotation_error = _rotation_distance_rad(observed[:3, :3], target_state.pose[:3, :3]) + path_converged = True + try: + self._verify_path_tracking( + env_id, + {eef_name: target_state.pose}, + joint_seed_names=target_state.joint_seed_names, + joint_seed=target_state.joint_seed, + ) + except AttemptGenerationError as exc: + if exc.code not in _RETRYABLE_TRACKING_FAILURE_CODES: + raise + path_converged = False + strict_converged = ( + math.isfinite(position_error) + and math.isfinite(rotation_error) + and position_error <= self.interaction_position_tolerance_m + and rotation_error <= self.interaction_rotation_tolerance_rad + ) + return position_error, rotation_error, path_converged, path_converged and strict_converged + + def _verify_terminal_targets_after_settling( + self, + env_id: int, + eef_names: Sequence[str], + *, + verification_samples: int, + ) -> None: + """Re-attest exact terminal targets after the final stability window.""" + + boundaries_by_eef = {str(item["eef_name"]): item for item in self._terminal_target_boundaries} + failures: list[str] = [] + for eef_name in eef_names: + target_state = self._last_cartesian_targets.get(eef_name) + if target_state is None: + continue + boundary = boundaries_by_eef[eef_name] + position_error, rotation_error, path_verified, converged = self._interaction_target_status( + env_id, + eef_name, + target_state, + ) + gripper_unchanged = self._gripper_values[eef_name] == boundary["gripper_value"] + passed = converged and gripper_unchanged + boundary["post_settle_verification"] = { + "gripper_unchanged": gripper_unchanged, + "joint_path_verified": path_verified, + "passed": passed, + "position_error_m": _finite_or_none(position_error), + "rotation_error_rad": _finite_or_none(rotation_error), + "verification_samples": verification_samples, + } + if not passed: + failures.append( + f"{eef_name!r} was {position_error:.6g}m / {rotation_error:.6g}rad from " + f"segment {target_state.segment_id!r} (joint_path_verified={path_verified}, " + f"gripper_unchanged={gripper_unchanged})" + ) + if failures: + raise AttemptGenerationError( + FailureStage.VERIFICATION, + "terminal_target_not_stable", + "terminal target stability was lost during final settling: " + "; ".join(failures), + ) + + def _hold(self, env_id: int, steps: int) -> None: + if steps <= 0: + return + poses = self.embodiment_adapter.get_eef_poses(env_ids=[env_id]) + for _ in range(steps): + targets = {name: pose[0] for name, pose in poses.items()} + self._step_targets(env_id, targets) + + def _step_target( + self, + env_id: int, + eef_name: str, + target: torch.Tensor, + *, + joint_seed_names: Sequence[str] = (), + joint_seed: Sequence[float] | None = None, + ) -> None: + # A collision-checked kinematic waypoint can take more than one simulator control step to + # track because the articulated robot has closed-loop dynamics. Keep the waypoint fixed + # while applying a small, bounded number of correction steps. This prevents tracking lag + # from accumulating across a dense plan while retaining a hard corridor and step budget. + for correction_index in range(self.max_tracking_correction_steps + 1): + try: + self._step_targets( + env_id, + {eef_name: target}, + joint_seed_names=joint_seed_names, + joint_seed=joint_seed, + ) + except AttemptGenerationError as exc: + if ( + exc.code not in _RETRYABLE_TRACKING_FAILURE_CODES + or correction_index >= self.max_tracking_correction_steps + ): + raise + self._tracking_correction_steps += 1 + else: + return + + def _step_targets( + self, + env_id: int, + targets: Mapping[str, torch.Tensor], + *, + joint_seed_names: Sequence[str] = (), + joint_seed: Sequence[float] | None = None, + ) -> None: + if self._step_count >= self.max_steps: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "execution_step_limit", + f"plan exceeded {self.max_steps} simulator steps", + ) + eef_names = tuple(self.embodiment_adapter.get_eef_names()) + if set(targets) != set(eef_names): + # The current executor is explicitly single-arm. This also prevents an accidental zero + # command on an unmentioned arm from moving a multi-arm embodiment. + if len(eef_names) != 1 or set(targets) != {eef_names[0]}: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "partial_multi_eef_action", + "executor requires targets for every end effector", + recoverable=False, + ) + grippers = { + name: torch.full( + (int(self.embodiment_adapter.gripper_action_dim),), + self._gripper_values[name], + dtype=torch.float32, + device=_device(self.env), + ) + for name in eef_names + } + action = self.embodiment_adapter.target_eef_pose_to_action( + target_eef_pose_dict=dict(targets), + gripper_action_dict=grippers, + action_noise_dict=None, + env_id=env_id, + ) + self._validate_raw_dik_action(action) + batch = torch.zeros(self.env.action_space.shape, dtype=action.dtype, device=_device(self.env)) + batch[env_id] = action + self._step_count += 1 + self.env.step(batch) + if self._pick_place_success is not None: + self._update_pick_place_success("observe_step", self._task_success_sample(env_id)) + self._verify_path_tracking( + env_id, + targets, + joint_seed_names=joint_seed_names, + joint_seed=joint_seed, + ) + + def _validate_raw_dik_action(self, action: torch.Tensor) -> None: + """Reject malformed or saturated normalized IK actions before ``env.step``.""" + + action = torch.as_tensor(action) + action_shape = tuple(int(size) for size in self.env.action_space.shape) + expected_action_dim = action_shape[-1] if action_shape else 0 + if action.ndim != 1 or action.shape[0] != expected_action_dim or not action.is_floating_point(): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "raw_dik_action_invalid", + f"adapter returned action shape/dtype {tuple(action.shape)}/{action.dtype}; " + f"expected one floating vector of width {expected_action_dim}", + recoverable=False, + ) + if not bool(torch.isfinite(action).all().item()): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "raw_dik_action_invalid", + "adapter returned a non-finite IK action", + recoverable=False, + ) + gripper_dim = int(self.embodiment_adapter.gripper_action_dim) + pose_dim = expected_action_dim - gripper_dim + if gripper_dim < 0 or pose_dim <= 0: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "raw_dik_action_invalid", + f"action width {expected_action_dim} does not leave a positive IK pose slice after " + f"{gripper_dim} gripper dimensions", + recoverable=False, + ) + maximum_pose_action = float(torch.max(torch.abs(action[:pose_dim])).item()) + if maximum_pose_action >= _RAW_DIK_POSE_ACTION_LIMIT - _RAW_DIK_SATURATION_TOLERANCE: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "dik_pose_action_saturated", + f"normalized IK pose action magnitude {maximum_pose_action:.6g} reached the clipping boundary; " + "the requested Cartesian step was not sent to the simulator", + ) + gripper = action[pose_dim:] + if gripper.numel() and bool(torch.any(torch.abs(gripper) > 1.0 + 1e-6).item()): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "raw_gripper_action_out_of_range", + "normalized gripper action must remain in [-1, 1]", + recoverable=False, + ) + + def _verify_path_tracking( + self, + env_id: int, + targets: Mapping[str, torch.Tensor], + *, + joint_seed_names: Sequence[str], + joint_seed: Sequence[float] | None, + ) -> None: + """Reject DIK divergence from the collision-checked Cartesian/joint reference path.""" + + observed_poses = self.embodiment_adapter.get_eef_poses(env_ids=[env_id]) + for eef_name, target in targets.items(): + if eef_name not in observed_poses: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "tracking_observation_missing", + f"no observed pose is available for end effector {eef_name!r}", + recoverable=False, + ) + observed = torch.as_tensor(observed_poses[eef_name][0], device=target.device, dtype=target.dtype) + position_error = float(torch.linalg.norm(observed[:3, 3] - target[:3, 3]).item()) + relative_rotation = observed[:3, :3].transpose(0, 1) @ target[:3, :3] + cosine = torch.clamp((torch.trace(relative_rotation) - 1.0) / 2.0, -1.0, 1.0) + rotation_error = float(torch.acos(cosine).item()) + if math.isfinite(position_error): + self._maximum_eef_position_error_m = max(self._maximum_eef_position_error_m, position_error) + if math.isfinite(rotation_error): + self._maximum_eef_rotation_error_rad = max( + self._maximum_eef_rotation_error_rad, + rotation_error, + ) + if not math.isfinite(position_error) or position_error > self.max_eef_position_error_m: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "eef_position_tracking_error", + f"{eef_name!r} position error {position_error:.6g}m exceeds {self.max_eef_position_error_m:.6g}m", + ) + if not math.isfinite(rotation_error) or rotation_error > self.max_eef_rotation_error_rad: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "eef_rotation_tracking_error", + f"{eef_name!r} rotation error {rotation_error:.6g}rad exceeds " + f"{self.max_eef_rotation_error_rad:.6g}rad", + ) + + if joint_seed is None: + return + observed_names = tuple(self.embodiment_adapter.get_joint_names()) + observed_positions = self.embodiment_adapter.get_joint_positions(env_ids=[env_id])[0] + observed_by_name = {name: float(observed_positions[index]) for index, name in enumerate(observed_names)} + missing = [name for name in joint_seed_names if name not in observed_by_name] + if missing: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "joint_tracking_observation_missing", + f"joint tracking observations are missing {missing}", + recoverable=False, + ) + joint_errors = [ + ( + abs(observed_by_name[name] - float(reference)), + name, + observed_by_name[name], + float(reference), + ) + for name, reference in zip(joint_seed_names, joint_seed) + ] + maximum_error, maximum_name, maximum_observed, maximum_reference = max( + joint_errors, + default=(0.0, "", 0.0, 0.0), + ) + if math.isfinite(maximum_error) and maximum_error > self._maximum_joint_path_error_rad: + self._maximum_joint_path_error_rad = maximum_error + self._maximum_joint_path_error_name = maximum_name + if not math.isfinite(maximum_error) or maximum_error > self.max_joint_path_error_rad: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "joint_path_tracking_error", + f"joint path error {maximum_error:.6g}rad on {maximum_name!r} exceeds " + f"{self.max_joint_path_error_rad:.6g}rad " + f"(observed={maximum_observed:.6g}, reference={maximum_reference:.6g})", + ) + + def _tracking_observation(self) -> dict[str, float | str | None]: + """Return bounded path-tracking evidence for the attempt record.""" + + return { + "maximum_eef_position_error_m": self._maximum_eef_position_error_m, + "maximum_eef_rotation_error_rad": self._maximum_eef_rotation_error_rad, + "maximum_joint_path_error_name": self._maximum_joint_path_error_name, + "maximum_joint_path_error_rad": self._maximum_joint_path_error_rad, + } + + def _physical_observation(self, env_id: int) -> dict[str, Any]: + """Return final physical state plus gripper-transition evidence for the attempt record.""" + + observation = { + "final_state": self._capture_physical_state(env_id), + "gripper_milestones": list(self._gripper_milestones), + "interaction_boundaries": [dict(item) for item in self._interaction_boundaries], + "interaction_correction_steps": self._interaction_correction_steps, + "terminal_correction_steps": self._terminal_correction_steps, + "terminal_target_boundaries": [dict(item) for item in self._terminal_target_boundaries], + } + if self._pick_place_success is not None: + eef_name = self._pick_place_success.eef_name + observation["task_success_report"] = self._pick_place_success.report( + logical_held_object=self.attachment_state.held_by_eef.get(eef_name) + ) + return observation + + def _task_success_sample(self, env_id: int) -> dict[str, Any]: + """Capture a required live sample for the fail-closed task-success checks.""" + + sample = self._capture_physical_state(env_id) + if "capture_error" in sample: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "task_success_state_unavailable", + str(sample["capture_error"]), + recoverable=False, + ) + return sample + + def _observe_task_success_final(self, env_id: int, *, goal_satisfied: bool) -> None: + """Record one post-settle task-success sample.""" + + self._update_pick_place_success( + "observe_final", + self._task_success_sample(env_id), + goal_satisfied=goal_satisfied, + failure_stage=FailureStage.VERIFICATION, + ) + + def _update_pick_place_success( + self, + method_name: str, + *args: Any, + failure_stage: FailureStage = FailureStage.EXECUTION, + **kwargs: Any, + ) -> Any: + """Convert malformed or unavailable task-success state into a classified failure.""" + + assert self._pick_place_success is not None + method = getattr(self._pick_place_success, method_name) + try: + return method(*args, **kwargs) + except AttemptGenerationError: + raise + except Exception as exc: + raise AttemptGenerationError( + failure_stage, + "task_success_state_unavailable", + f"{method_name} failed: {type(exc).__name__}: {str(exc)[:1024]}", + recoverable=False, + ) from exc + + def _capture_physical_state(self, env_id: int) -> dict[str, Any]: + """Capture a compact environment-origin observation without mutating simulator state.""" + + try: + origin = _env_origin(self.env, env_id) + eef_poses = { + name: [list(row) for row in _matrix_to_tuple(pose[0])] + for name, pose in self.embodiment_adapter.get_eef_poses(env_ids=[env_id]).items() + } + objects = {} + for name, scene_object in sorted(self.env.scene.rigid_objects.items()): + position = _tensor_row(scene_object.data.root_pos_w, env_id) - origin + quaternion = _tensor_row(scene_object.data.root_quat_w, env_id) + object_observation = { + "position_env_m": [float(value) for value in position.tolist()], + "quaternion_xyzw": [float(value) for value in quaternion.tolist()], + } + linear_velocity = getattr(scene_object.data, "root_lin_vel_w", None) + angular_velocity = getattr(scene_object.data, "root_ang_vel_w", None) + if linear_velocity is not None: + object_observation["linear_speed_m_s"] = float( + torch.linalg.norm(_tensor_row(linear_velocity, env_id)).item() + ) + if angular_velocity is not None: + object_observation["angular_speed_rad_s"] = float( + torch.linalg.norm(_tensor_row(angular_velocity, env_id)).item() + ) + objects[name] = object_observation + joint_names = tuple(self.embodiment_adapter.get_joint_names()) + joint_positions = self.embodiment_adapter.get_joint_positions(env_ids=[env_id])[0] + return { + "eef_poses_env": eef_poses, + "gripper_commands": dict(self._gripper_values), + "held_objects": dict(self.attachment_state.held_by_eef), + "joint_positions": {name: float(joint_positions[index]) for index, name in enumerate(joint_names)}, + "objects": objects, + } + except Exception as exc: + return { + "capture_error": f"{type(exc).__name__}: {str(exc)[:512]}", + } + + def _verify_attachment_proximity(self, env_id: int, eef_name: str, object_name: str) -> bool: + if self._gripper_values[eef_name] >= 0: + return False + scene_object = self.env.scene.rigid_objects.get(object_name) + if scene_object is None: + return False + object_position = _tensor_row(scene_object.data.root_pos_w, env_id) - _env_origin(self.env, env_id) + eef_pose = self.embodiment_adapter.get_eef_poses(env_ids=[env_id])[eef_name][0] + distance = torch.linalg.norm(object_position - eef_pose[:3, 3]).item() + return math.isfinite(distance) and distance <= self.attachment_distance_m + + def _require_eef(self, name: str) -> None: + if name not in self._gripper_values: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unknown_end_effector", + f"plan references unknown end effector {name!r}", + recoverable=False, + ) + + def _event( + self, + request: AttemptRequest, + plan: TaskMotionPlan, + segment_id: str | None, + event_type: ExecutionEventType, + outcome: ExecutionOutcome, + *, + failure_code: str | None = None, + message: str | None = None, + ) -> ExecutionEvent: + self._event_count += 1 + return ExecutionEvent( + event_id=make_stable_id( + "event", + request.attempt_id, + plan.plan_id, + segment_id, + event_type.value, + self._event_count, + ), + attempt_id=request.attempt_id, + plan_id=plan.plan_id, + segment_id=segment_id, + event_type=event_type, + outcome=outcome, + monotonic_time_s=time.monotonic(), + failure_code=failure_code, + message=message, + metadata={"step": self._step_count}, + ) + + +def success_term_verifier(success_term: Any) -> SuccessVerifier: + """Wrap a saved Isaac Lab ``TerminationTermCfg`` as a final-state verifier.""" + + if success_term is None or not callable(getattr(success_term, "func", None)): + raise ValueError("success_term must expose a callable func") + params = dict(getattr(success_term, "params", {}) or {}) + + def verify(env: Any, _env_id: int) -> Any: + return success_term.func(env, **params) + + return verify + + +def _base_env(env: Any) -> Any: + return getattr(env, "unwrapped", env) + + +def _device(env: Any) -> torch.device: + return torch.device(getattr(env, "device", "cpu")) + + +def _env_id_tensor(env: Any, env_id: int) -> torch.Tensor: + if env_id < 0 or env_id >= int(env.num_envs): + raise ValueError(f"env_id {env_id} outside [0, {env.num_envs})") + return torch.tensor([env_id], dtype=torch.int64, device=_device(env)) + + +def _tensor_row(value: Any, env_id: int) -> torch.Tensor: + tensor = torch.as_tensor(value) + return tensor[env_id].detach() + + +def _row_to_tuple(value: Any) -> tuple[float, ...]: + tensor = torch.as_tensor(value).detach().cpu().reshape(-1) + result = tuple(float(item) for item in tensor.tolist()) + if not all(math.isfinite(item) for item in result): + raise ValueError("snapshot tensor contains non-finite values") + return result + + +def _matrix_to_tuple(value: Any) -> tuple[tuple[float, float, float, float], ...]: + tensor = torch.as_tensor(value).detach().cpu() + if tensor.shape != (4, 4): + raise ValueError(f"pose must have shape (4, 4), got {tuple(tensor.shape)}") + rows = tuple(tuple(float(item) for item in row) for row in tensor.tolist()) + return rows # type: ignore[return-value] + + +def _pose_from_xyzw(position: torch.Tensor, quaternion: torch.Tensor) -> torch.Tensor: + if position.shape != (3,) or quaternion.shape != (4,): + raise ValueError("root position/quaternion must have shapes (3,) and (4,)") + if not bool(torch.isfinite(position).all().item()) or not bool(torch.isfinite(quaternion).all().item()): + raise ValueError("root position/quaternion must be finite") + norm = torch.linalg.norm(quaternion) + if float(norm.item()) <= 1e-12: + raise ValueError("root quaternion must have nonzero norm") + quaternion = quaternion / norm + x, y, z, w = quaternion.unbind() + rotation = torch.stack(( + 1 - 2 * (y * y + z * z), + 2 * (x * y - z * w), + 2 * (x * z + y * w), + 2 * (x * y + z * w), + 1 - 2 * (x * x + z * z), + 2 * (y * z - x * w), + 2 * (x * z - y * w), + 2 * (y * z + x * w), + 1 - 2 * (x * x + y * y), + )).reshape(3, 3) + pose = torch.eye(4, dtype=position.dtype, device=position.device) + pose[:3, :3] = rotation + pose[:3, 3] = position + return pose + + +def _validate_raw_dik_action(env: Any, embodiment_adapter: Any, action: Any, *, context: str) -> None: + """Validate one normalized delta-IK action without mutating simulator state.""" + + action = torch.as_tensor(action) + action_shape = tuple(int(size) for size in env.action_space.shape) + expected_action_dim = action_shape[-1] if action_shape else 0 + if action.ndim != 1 or action.shape[0] != expected_action_dim or not action.is_floating_point(): + raise ValueError( + f"{context} adapter returned action shape/dtype {tuple(action.shape)}/{action.dtype}; " + f"expected one floating vector of width {expected_action_dim}" + ) + if not bool(torch.isfinite(action).all().item()): + raise ValueError(f"{context} adapter returned a non-finite IK action") + gripper_dim = int(embodiment_adapter.gripper_action_dim) + pose_dim = expected_action_dim - gripper_dim + if gripper_dim < 0 or pose_dim <= 0: + raise ValueError( + f"{context} action width {expected_action_dim} does not leave a positive IK pose slice after " + f"{gripper_dim} gripper dimensions" + ) + maximum_pose_action = float(torch.max(torch.abs(action[:pose_dim])).item()) + if maximum_pose_action >= _RAW_DIK_POSE_ACTION_LIMIT - _RAW_DIK_SATURATION_TOLERANCE: + raise ValueError( + f"{context} normalized IK pose action magnitude {maximum_pose_action:.6g} reached the clipping boundary; " + "the requested Cartesian step was not sent to the simulator" + ) + gripper = action[pose_dim:] + if gripper.numel() and bool(torch.any(torch.abs(gripper) > 1.0 + 1e-6).item()): + raise ValueError(f"{context} normalized gripper action must remain in [-1, 1]") + + +def _env_origin(env: Any, env_id: int) -> torch.Tensor: + return torch.as_tensor(env.scene.env_origins)[env_id].detach() + + +def _step_dt(env: Any) -> float: + value = getattr(env, "step_dt", None) + if value is None: + value = getattr(getattr(env, "cfg", None), "decimation", 1) * getattr(env.sim, "physics_dt", 0.0) + value = float(value) + if not math.isfinite(value) or value <= 0: + raise ValueError("environment step_dt must be finite and positive") + return value + + +def _cartesian_step_dt(segment: CartesianTrajectorySegment, env: Any) -> float: + """Attest the planner sampling interval and return the live control interval [s].""" + + control_dt_s = _step_dt(env) + value = segment.metadata.get("step_dt_s") + if value is None: + return control_dt_s + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_step_dt_invalid", + f"segment {segment.segment_id!r} step_dt_s must be a finite positive number", + recoverable=False, + ) + step_dt_s = float(value) + if not math.isfinite(step_dt_s) or step_dt_s <= 0: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_step_dt_invalid", + f"segment {segment.segment_id!r} step_dt_s must be a finite positive number", + recoverable=False, + ) + if not math.isclose(step_dt_s, control_dt_s, rel_tol=1e-6, abs_tol=1e-9): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "cartesian_step_dt_mismatch", + f"segment {segment.segment_id!r} declares step_dt_s={step_dt_s:.9g}, but the live control " + f"interval is {control_dt_s:.9g}s", + recoverable=False, + ) + return control_dt_s + + +def _rotation_distance_rad(first: torch.Tensor, second: torch.Tensor) -> float: + """Return the geodesic angle [rad] between two rotation matrices.""" + + relative = first.transpose(0, 1) @ second + cosine = torch.clamp((torch.trace(relative) - 1.0) / 2.0, -1.0, 1.0) + return float(torch.acos(cosine).item()) + + +def _finite_at_most(value: float, maximum: float) -> bool: + """Apply one physical upper bound with only float32 roundoff tolerance.""" + + return math.isfinite(value) and ( + value <= maximum or math.isclose(value, maximum, rel_tol=0.0, abs_tol=_PHYSICAL_BOUNDARY_ABS_TOLERANCE) + ) + + +def _finite_or_none(value: float) -> float | None: + """Return a finite run-record measurement or ``None`` when unavailable.""" + + return value if math.isfinite(value) else None + + +def _robot_base_position_in_env(env: Any, env_id: int) -> torch.Tensor: + """Return the sole live articulation root in environment-origin coordinates [m].""" + + articulations = getattr(env.scene, "articulations", None) + if not isinstance(articulations, Mapping) or len(articulations) != 1: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "workspace_frame_unavailable", + "Franka workspace validation requires exactly one live articulation root", + recoverable=False, + ) + articulation = next(iter(articulations.values())) + root_pos_w = getattr(getattr(articulation, "data", None), "root_pos_w", None) + if root_pos_w is None: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "workspace_frame_unavailable", + "Franka workspace validation requires live articulation root_pos_w", + recoverable=False, + ) + try: + position = torch.as_tensor(root_pos_w, dtype=torch.float64, device=_device(env))[env_id] + position = position - torch.as_tensor(_env_origin(env, env_id), dtype=torch.float64, device=_device(env)) + except Exception as exc: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "workspace_frame_unavailable", + f"failed to read the live articulation root: {type(exc).__name__}: {str(exc)[:512]}", + recoverable=False, + ) from exc + if position.shape != (3,) or not bool(torch.isfinite(position).all().item()): + raise AttemptGenerationError( + FailureStage.EXECUTION, + "workspace_frame_unavailable", + "live articulation root position must be a finite 3-vector", + recoverable=False, + ) + return position + + +def _geometry_ref(scene_object: Any) -> str | None: + value = getattr(getattr(getattr(scene_object, "cfg", None), "spawn", None), "usd_path", None) + if value is None: + return None + value = str(value) + return value if len(value) <= 2048 else None + + +def _target_in_env_frame(env: Any, env_id: int, target: torch.Tensor, frame: str) -> torch.Tensor: + if frame in ("env", "env_origin"): + return target + if frame == "world": + result = target.clone() + result[:3, 3] -= _env_origin(env, env_id) + return result + raise AttemptGenerationError( + FailureStage.EXECUTION, + "unsupported_pose_frame", + f"executor supports only 'env_origin' and 'world' poses, got {frame!r}", + recoverable=False, + ) + + +def _gripper_value(segment: GripperCommandSegment) -> float: + if segment.command is GripperCommandMode.OPEN: + return 1.0 + if segment.command is GripperCommandMode.CLOSE: + return -1.0 + if segment.command is GripperCommandMode.POSITION: + assert segment.value is not None + if not -1.0 <= segment.value <= 1.0: + raise AttemptGenerationError( + FailureStage.EXECUTION, + "gripper_position_out_of_range", + f"normalized gripper position must be in [-1, 1], got {segment.value}", + recoverable=False, + ) + return segment.value + raise AttemptGenerationError( + FailureStage.EXECUTION, + "gripper_effort_not_supported", + "the binary-gripper executor does not support effort commands", + recoverable=False, + ) + + +def _env_bool(value: bool | Sequence[bool] | torch.Tensor, env_id: int) -> bool: + if isinstance(value, bool): + return value + tensor = torch.as_tensor(value, dtype=torch.bool).reshape(-1) + if env_id >= tensor.numel(): + raise ValueError(f"success verifier returned only {tensor.numel()} environment results") + return bool(tensor[env_id].item()) diff --git a/isaac_autodata_interfaces/autonomous/pick_place_success.py b/isaac_autodata_interfaces/autonomous/pick_place_success.py new file mode 100644 index 0000000..fe6aa77 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/pick_place_success.py @@ -0,0 +1,904 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Live pick-and-place success checks for the reviewed Franka cube-into-bowl profile.""" + +from __future__ import annotations + +import math +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from itertools import takewhile +from typing import Any + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + DetachIntentSegment, + GripperCommandMode, + GripperCommandSegment, + TaskMotionPlan, + matrix4_inverse, + matrix4_multiply, +) +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import ( + FRANKA_PICK_CUBE_INTO_BOWL, + PickPlaceSuccessThresholds, +) + +_TASK_PROFILE = FRANKA_PICK_CUBE_INTO_BOWL +_SCHEDULESTREAM_BACKENDS = frozenset({ + _TASK_PROFILE.schedulestream_plan_backend, + "schedulestream_custream2", +}) + + +@dataclass(frozen=True) +class _PhysicalSample: + subject_position: tuple[float, float, float] + subject_aabb_center_position: tuple[float, float, float] + subject_rotation: tuple[tuple[float, float, float], ...] + subject_linear_speed: float + subject_angular_speed: float + target_position: tuple[float, float, float] + target_aabb_center_position: tuple[float, float, float] + eef_position: tuple[float, float, float] + eef_rotation: tuple[tuple[float, float, float], ...] + finger_aperture: float + + +class PickPlaceSuccessTracker: + """Accumulate live observations and check one physical pick-and-place execution.""" + + schema_version = 3 + profile = _TASK_PROFILE.success_profile + + def __init__( + self, + *, + subject: str, + target: str, + eef_name: str, + initial_open_segment_id: str, + close_segment_id: str, + attach_segment_id: str, + release_open_segment_id: str, + detach_segment_id: str, + success_contract: Mapping[str, Any], + grasp_geometry: Mapping[str, Any], + placement_geometry: Mapping[str, Any], + subject_aabb_center_offset_m: tuple[float, float, float], + target_aabb_center_offset_m: tuple[float, float, float], + thresholds: PickPlaceSuccessThresholds | None = None, + ) -> None: + self.subject = subject + self.target = target + self.eef_name = eef_name + self.initial_open_segment_id = initial_open_segment_id + self.close_segment_id = close_segment_id + self.attach_segment_id = attach_segment_id + self.release_open_segment_id = release_open_segment_id + self.detach_segment_id = detach_segment_id + self.success_contract = dict(success_contract) + self.grasp_geometry = dict(grasp_geometry) + self.placement_geometry = dict(placement_geometry) + self.subject_aabb_center_offset_m = subject_aabb_center_offset_m + self.target_aabb_center_offset_m = target_aabb_center_offset_m + self.thresholds = thresholds or _TASK_PROFILE.success_thresholds + self._baseline: _PhysicalSample | None = None + self._baseline_goal_satisfied: bool | None = None + self._attachment_candidate: _PhysicalSample | None = None + self._attach: _PhysicalSample | None = None + self._release: _PhysicalSample | None = None + self._last: _PhysicalSample | None = None + self._attached = False + self._release_started = False + self._detached = False + self._closed_samples = 0 + self._maximum_lift_m = 0.0 + self._maximum_subject_displacement_m = 0.0 + self._maximum_eef_displacement_m = 0.0 + self._maximum_relative_translation_drift_m = 0.0 + self._maximum_relative_rotation_drift_rad = 0.0 + self._maximum_destination_drift_m = 0.0 + self._final_samples: list[tuple[_PhysicalSample, bool]] = [] + + @classmethod + def from_plan( + cls, + plan: TaskMotionPlan, + eef_names: Sequence[str], + *, + thresholds: PickPlaceSuccessThresholds | None = None, + ) -> PickPlaceSuccessTracker | None: + """Return a tracker for a ScheduleStream place plan, validating its full lifecycle.""" + + if plan.backend not in _SCHEDULESTREAM_BACKENDS: + return None + if len(plan.goal) != 1: + raise ValueError("the reviewed ScheduleStream profile requires exactly one semantic goal") + goal = plan.goal[0] + if goal.relation.lower() != "on" or goal.target is None: + raise ValueError("the reviewed ScheduleStream profile requires exactly one binary 'on' goal") + metadata = plan.metadata.get("schedulestream") + if not isinstance(metadata, dict) or metadata.get("attachment_events_preserved") is not True: + raise ValueError("ScheduleStream plan does not attest preserved attachment events") + grasp_geometry = _validate_grasp_geometry(metadata.get("grasp_geometry"), goal.subject) + placement_geometry = _validate_placement_geometry( + metadata.get("destination_placement"), goal.subject, goal.target + ) + if grasp_geometry["object_from_aabb"] != placement_geometry["subject_object_from_aabb"]: + raise ValueError("ScheduleStream grasp and placement AABB transforms do not match") + success_contract = plan.metadata.get("arena_success_contract") + if not isinstance(success_contract, dict) or success_contract.get("attested") is not True: + raise ValueError("ScheduleStream plan does not carry the attested Arena success contract") + + indexed = list(enumerate(plan.segments)) + grippers = [(index, segment) for index, segment in indexed if isinstance(segment, GripperCommandSegment)] + attaches = [(index, segment) for index, segment in indexed if isinstance(segment, AttachIntentSegment)] + detaches = [(index, segment) for index, segment in indexed if isinstance(segment, DetachIntentSegment)] + states = tuple(segment.command for _, segment in grippers) + if states != (GripperCommandMode.OPEN, GripperCommandMode.CLOSE, GripperCommandMode.OPEN): + raise ValueError("ScheduleStream place plan must contain exactly open-close-open gripper commands") + if len(attaches) != 1 or len(detaches) != 1: + raise ValueError("ScheduleStream place plan must contain exactly one attach and one detach intent") + + initial_index, initial_open = grippers[0] + close_index, close = grippers[1] + release_index, release_open = grippers[2] + attach_index, attach = attaches[0] + detach_index, detach = detaches[0] + if not initial_index < close_index < attach_index < release_index < detach_index: + raise ValueError("ScheduleStream place lifecycle must be ordered open-close-attach-open-detach") + if not any( + isinstance(segment, CartesianTrajectorySegment) + for segment in plan.segments[attach_index + 1 : release_index] + ): + raise ValueError("ScheduleStream place plan must transport the object between attach and release") + if attach.object_name != goal.subject or detach.object_name != goal.subject: + raise ValueError("ScheduleStream attachment intents must bind exactly to the goal subject") + lifecycle_eefs = { + initial_open.eef_name, + close.eef_name, + attach.eef_name, + release_open.eef_name, + detach.eef_name, + } + if len(lifecycle_eefs) != 1 or lifecycle_eefs != set(eef_names): + raise ValueError("ScheduleStream place lifecycle must bind exactly to the sole live end effector") + if attach.verifier != "contact_and_relative_motion_v1" or detach.verifier != "contact_and_relative_motion_v1": + raise ValueError("ScheduleStream attachment lifecycle uses an unreviewed physical verifier") + + return cls( + subject=goal.subject, + target=goal.target, + eef_name=attach.eef_name, + initial_open_segment_id=initial_open.segment_id, + close_segment_id=close.segment_id, + attach_segment_id=attach.segment_id, + release_open_segment_id=release_open.segment_id, + detach_segment_id=detach.segment_id, + success_contract=success_contract, + grasp_geometry=grasp_geometry["geometry"], + placement_geometry=placement_geometry["geometry"], + subject_aabb_center_offset_m=placement_geometry["subject_offset"], + target_aabb_center_offset_m=placement_geometry["target_offset"], + thresholds=thresholds, + ) + + def settle_initial(self, segment_id: str, sample: Mapping[str, Any], *, goal_satisfied: bool) -> None: + """Capture the post-open settled baseline and prove the task was not initially complete.""" + + if segment_id != self.initial_open_segment_id or self._baseline is not None: + return + self._baseline = self._read_sample(sample) + self._last = self._baseline + self._baseline_goal_satisfied = goal_satisfied + + def attach(self, segment_id: str, object_name: str, eef_name: str, sample: Mapping[str, Any]) -> None: + """Start the physical closed-transport window after aperture/distance verification.""" + + if segment_id != self.attach_segment_id or object_name != self.subject or eef_name != self.eef_name: + raise ValueError("executed attach intent does not match the attested semantic lifecycle") + if self._baseline is None or self._attach is not None: + raise ValueError("attach intent occurred without one settled initial baseline") + self._attach = self._read_sample(sample) + self._attachment_candidate = self._attach + if not self._attachment_sample_is_plausible(self._attach): + self._attach = None + raise ValueError("attach intent lacks plausible physical aperture/distance evidence") + self._last = self._attach + self._attached = True + + def attachment_is_plausible(self, sample: Mapping[str, Any]) -> bool: + """Return whether a prospective attach has finite, asset-profiled grasp evidence.""" + + self._attachment_candidate = self._read_sample(sample) + return self._attachment_sample_is_plausible(self._attachment_candidate) + + def _attachment_sample_is_plausible(self, sample: _PhysicalSample) -> bool: + distance = _distance(sample.subject_position, sample.eef_position) + return ( + distance <= self.thresholds.attachment_distance_m + and self.thresholds.grasp_aperture_min_m <= sample.finger_aperture <= self.thresholds.grasp_aperture_max_m + ) + + def observe_step(self, sample: Mapping[str, Any]) -> None: + """Observe one completed simulator step, including closed-transport extrema.""" + + current = self._read_sample(sample) + self._last = current + if self._baseline is not None: + self._maximum_destination_drift_m = max( + self._maximum_destination_drift_m, + _distance(current.target_aabb_center_position, self._baseline.target_aabb_center_position), + ) + if not self._attached or self._release_started: + return + assert self._attach is not None + self._closed_samples += 1 + self._maximum_lift_m = max( + self._maximum_lift_m, + current.subject_position[2] - self._attach.subject_position[2], + ) + self._maximum_subject_displacement_m = max( + self._maximum_subject_displacement_m, + _distance(current.subject_position, self._attach.subject_position), + ) + self._maximum_eef_displacement_m = max( + self._maximum_eef_displacement_m, + _distance(current.eef_position, self._attach.eef_position), + ) + attach_translation, attach_rotation = _relative_object_pose(self._attach) + current_translation, current_rotation = _relative_object_pose(current) + self._maximum_relative_translation_drift_m = max( + self._maximum_relative_translation_drift_m, + _distance(current_translation, attach_translation), + ) + self._maximum_relative_rotation_drift_rad = max( + self._maximum_relative_rotation_drift_rad, + _rotation_distance(current_rotation, attach_rotation), + ) + + def begin_release(self, segment_id: str, sample: Mapping[str, Any]) -> None: + """Close the transport window before the gripper begins opening.""" + + if segment_id != self.release_open_segment_id or not self._attached or self._release_started: + raise ValueError("release command does not match the attested semantic lifecycle") + self._release = self._read_sample(sample) + self._last = self._release + self._release_started = True + + def release_contact_gate(self, segment_id: str, sample: Mapping[str, Any]) -> dict[str, Any]: + """Prove semantic placement and closed transport before the gripper may open. + + This gate is evaluated at the attested release-open boundary while the gripper is still + closed. It intentionally does not apply the final settled speed thresholds: contact with + the destination can leave the subject moving before release, and final verification owns + the stable-state decision. + """ + + if segment_id != self.release_open_segment_id or not self._attached or self._release_started: + raise ValueError("release contact gate does not match the attested semantic lifecycle") + if self._baseline is None or self._attach is None: + raise ValueError("release contact gate requires settled baseline and attachment evidence") + current = self._read_sample(sample) + delta = tuple( + current.subject_aabb_center_position[index] - current.target_aabb_center_position[index] + for index in range(3) + ) + horizontal_radius = math.hypot(delta[0], delta[1]) + vertical_offset_abs = abs(delta[2]) + current_destination_drift = _distance( + current.target_aabb_center_position, + self._baseline.target_aabb_center_position, + ) + accumulated_destination_drift = max(self._maximum_destination_drift_m, current_destination_drift) + current_attachment_distance = _distance(current.subject_position, current.eef_position) + thresholds = self.thresholds + checks = ( + _maximum_check( + "prerelease_subject_target_horizontal_radius_m", + horizontal_radius, + thresholds.maximum_final_horizontal_radius_m, + ), + _maximum_check( + "prerelease_subject_target_vertical_offset_abs_m", + vertical_offset_abs, + thresholds.maximum_final_vertical_offset_m, + ), + _maximum_check( + "prerelease_maximum_destination_drift_m", + accumulated_destination_drift, + thresholds.maximum_destination_drift_m, + ), + _maximum_check( + "prerelease_subject_eef_attachment_distance_m", + current_attachment_distance, + thresholds.attachment_distance_m, + ), + _minimum_check( + "prerelease_grasp_aperture_min_m", + current.finger_aperture, + thresholds.grasp_aperture_min_m, + ), + _maximum_check( + "prerelease_grasp_aperture_max_m", + current.finger_aperture, + thresholds.grasp_aperture_max_m, + ), + _minimum_check( + "prerelease_closed_transport_samples", self._closed_samples, thresholds.minimum_closed_samples + ), + _minimum_check("prerelease_closed_transport_lift_m", self._maximum_lift_m, thresholds.minimum_lift_m), + _minimum_check( + "prerelease_closed_transport_subject_displacement_m", + self._maximum_subject_displacement_m, + thresholds.minimum_transport_m, + ), + _minimum_check( + "prerelease_closed_transport_eef_displacement_m", + self._maximum_eef_displacement_m, + thresholds.minimum_transport_m, + ), + _maximum_check( + "prerelease_closed_transport_relative_translation_drift_m", + self._maximum_relative_translation_drift_m, + thresholds.maximum_relative_translation_drift_m, + ), + _maximum_check( + "prerelease_closed_transport_relative_rotation_drift_rad", + self._maximum_relative_rotation_drift_rad, + thresholds.maximum_relative_rotation_drift_rad, + ), + ) + return { + "checks": list(checks), + "closed_transport": { + "maximum_eef_displacement_m": self._maximum_eef_displacement_m, + "maximum_lift_m": self._maximum_lift_m, + "maximum_relative_rotation_drift_rad": self._maximum_relative_rotation_drift_rad, + "maximum_relative_translation_drift_m": self._maximum_relative_translation_drift_m, + "maximum_subject_displacement_m": self._maximum_subject_displacement_m, + "samples": self._closed_samples, + }, + "current_physical_state_finite": True, + "current_prerelease_placement": { + "grasp_aperture_m": current.finger_aperture, + "subject_eef_attachment_distance_m": current_attachment_distance, + "subject_target_horizontal_radius_m": horizontal_radius, + "subject_target_vertical_offset_abs_m": vertical_offset_abs, + }, + "maximum_destination_drift_m": accumulated_destination_drift, + "passed": all(check["passed"] for check in checks), + "segment_id": segment_id, + } + + def detach(self, segment_id: str, object_name: str, eef_name: str, sample: Mapping[str, Any]) -> None: + """Record completion of the logical detach after physical opening settles.""" + + if ( + segment_id != self.detach_segment_id + or object_name != self.subject + or eef_name != self.eef_name + or not self._release_started + ): + raise ValueError("executed detach intent does not match the attested semantic lifecycle") + self._last = self._read_sample(sample) + self._attached = False + self._detached = True + + def observe_final(self, sample: Mapping[str, Any], *, goal_satisfied: bool) -> None: + """Record one final-state verification sample.""" + + current = self._read_sample(sample) + self._last = current + self._final_samples.append((current, goal_satisfied)) + if self._baseline is not None: + self._maximum_destination_drift_m = max( + self._maximum_destination_drift_m, + _distance(current.target_aabb_center_position, self._baseline.target_aabb_center_position), + ) + + def report(self, *, logical_held_object: str | None) -> dict[str, Any]: + """Return bounded evidence, checks, and the final pass/fail decision.""" + + thresholds = self.thresholds + final = self._final_samples[-1][0] if self._final_samples else self._last + final_success_streak = sum(1 for _ in takewhile(lambda item: item[1], reversed(self._final_samples))) + + checks: list[dict[str, Any]] = [] + + def maximum(name: str, observed: float | int | None, threshold: float | int) -> None: + passed = observed is not None and observed <= threshold + checks.append( + {"name": name, "observed": observed, "operator": "<=", "passed": passed, "threshold": threshold} + ) + + def minimum(name: str, observed: float | int | None, threshold: float | int) -> None: + passed = observed is not None and observed >= threshold + checks.append( + {"name": name, "observed": observed, "operator": ">=", "passed": passed, "threshold": threshold} + ) + + def exact(name: str, observed: Any, expected: Any) -> None: + checks.append({ + "name": name, + "observed": observed, + "operator": "==", + "passed": observed == expected, + "threshold": expected, + }) + + attach_distance = None + attach_aperture = None + grasp_sample = self._attach if self._attach is not None else self._attachment_candidate + if grasp_sample is not None: + attach_distance = _distance(grasp_sample.subject_position, grasp_sample.eef_position) + attach_aperture = grasp_sample.finger_aperture + final_linear_speed = None + final_angular_speed = None + final_horizontal_radius = None + final_vertical_offset = None + final_eef_separation = None + final_aperture = None + final_subject_displacement = None + if final is not None: + final_linear_speed = max( + (sample.subject_linear_speed for sample, _ in self._final_samples), + default=final.subject_linear_speed, + ) + final_angular_speed = max( + (sample.subject_angular_speed for sample, _ in self._final_samples), + default=final.subject_angular_speed, + ) + delta = tuple( + final.subject_aabb_center_position[index] - final.target_aabb_center_position[index] + for index in range(3) + ) + final_horizontal_radius = math.hypot(delta[0], delta[1]) + final_vertical_offset = delta[2] + final_eef_separation = _distance(final.subject_position, final.eef_position) + final_aperture = final.finger_aperture + if self._baseline is not None: + final_subject_displacement = _distance(final.subject_position, self._baseline.subject_position) + + exact("initial_goal_false", self._baseline_goal_satisfied, False) + exact("attach_observed", self._attach is not None, True) + maximum("attach_distance_m", attach_distance, thresholds.attachment_distance_m) + minimum("grasp_aperture_min_m", attach_aperture, thresholds.grasp_aperture_min_m) + maximum("grasp_aperture_max_m", attach_aperture, thresholds.grasp_aperture_max_m) + minimum("closed_transport_samples", self._closed_samples, thresholds.minimum_closed_samples) + minimum("closed_transport_lift_m", self._maximum_lift_m, thresholds.minimum_lift_m) + minimum( + "closed_transport_subject_displacement_m", + self._maximum_subject_displacement_m, + thresholds.minimum_transport_m, + ) + minimum("closed_transport_eef_displacement_m", self._maximum_eef_displacement_m, thresholds.minimum_transport_m) + maximum( + "closed_transport_relative_translation_drift_m", + self._maximum_relative_translation_drift_m, + thresholds.maximum_relative_translation_drift_m, + ) + maximum( + "closed_transport_relative_rotation_drift_rad", + self._maximum_relative_rotation_drift_rad, + thresholds.maximum_relative_rotation_drift_rad, + ) + exact("release_observed", self._release is not None, True) + exact("detach_observed", self._detached, True) + minimum("final_arena_success_streak", final_success_streak, thresholds.minimum_success_streak) + maximum("final_subject_linear_speed_m_s", final_linear_speed, thresholds.maximum_final_linear_speed_m_s) + maximum("final_subject_angular_speed_rad_s", final_angular_speed, thresholds.maximum_final_angular_speed_rad_s) + maximum( + "final_subject_target_horizontal_radius_m", + final_horizontal_radius, + thresholds.maximum_final_horizontal_radius_m, + ) + maximum( + "final_subject_target_vertical_offset_abs_m", + None if final_vertical_offset is None else abs(final_vertical_offset), + thresholds.maximum_final_vertical_offset_m, + ) + maximum( + "maximum_destination_drift_m", + self._maximum_destination_drift_m, + thresholds.maximum_destination_drift_m, + ) + minimum("final_eef_subject_separation_m", final_eef_separation, thresholds.minimum_final_eef_separation_m) + minimum("final_gripper_aperture_m", final_aperture, thresholds.minimum_final_aperture_m) + minimum("final_subject_displacement_m", final_subject_displacement, thresholds.minimum_transport_m) + exact("logical_attachment_empty", logical_held_object, None) + + return { + "checks": checks, + "closed_transport": { + "maximum_eef_displacement_m": self._maximum_eef_displacement_m, + "maximum_lift_m": self._maximum_lift_m, + "maximum_relative_rotation_drift_rad": self._maximum_relative_rotation_drift_rad, + "maximum_relative_translation_drift_m": self._maximum_relative_translation_drift_m, + "maximum_subject_displacement_m": self._maximum_subject_displacement_m, + "samples": self._closed_samples, + }, + "geometry_models": { + "grasp_geometry": dict(self.grasp_geometry), + "placement_geometry": dict(self.placement_geometry), + "position_metric": "rotated_local_aabb_centers_in_env_origin", + "source": "attested_schedulestream_converted_geometry", + }, + "arena_success_contract": dict(self.success_contract), + "goal": {"relation": "on", "subject": self.subject, "target": self.target}, + "maximum_destination_drift_m": self._maximum_destination_drift_m, + "passed": all(check["passed"] for check in checks), + "plan_lifecycle": { + "attach_segment_id": self.attach_segment_id, + "close_segment_id": self.close_segment_id, + "detach_segment_id": self.detach_segment_id, + "initial_open_segment_id": self.initial_open_segment_id, + "release_open_segment_id": self.release_open_segment_id, + }, + "profile": self.profile, + "schema_version": self.schema_version, + "thresholds": {key: value for key, value in vars(thresholds).items()}, + } + + def _read_sample(self, sample: Mapping[str, Any]) -> _PhysicalSample: + objects = _mapping(sample.get("objects"), "objects") + subject = _mapping(objects.get(self.subject), f"objects.{self.subject}") + target = _mapping(objects.get(self.target), f"objects.{self.target}") + eef_poses = _mapping(sample.get("eef_poses_env"), "eef_poses_env") + eef_pose = _matrix4(eef_poses.get(self.eef_name), f"eef_poses_env.{self.eef_name}") + joint_positions = _mapping(sample.get("joint_positions"), "joint_positions") + aperture = sum( + _finite(joint_positions.get(name), f"joint_positions.{name}") for name in _TASK_PROFILE.finger_joint_names + ) + subject_position = _vector3(subject.get("position_env_m"), f"objects.{self.subject}.position_env_m") + subject_rotation = _quaternion_rotation( + subject.get("quaternion_xyzw"), + f"objects.{self.subject}.quaternion_xyzw", + ) + target_position = _vector3(target.get("position_env_m"), f"objects.{self.target}.position_env_m") + target_rotation = _quaternion_rotation( + target.get("quaternion_xyzw"), + f"objects.{self.target}.quaternion_xyzw", + ) + return _PhysicalSample( + subject_position=subject_position, + subject_aabb_center_position=_apply_local_offset( + subject_position, + subject_rotation, + self.subject_aabb_center_offset_m, + ), + subject_rotation=subject_rotation, + subject_linear_speed=_nonnegative( + subject.get("linear_speed_m_s"), + f"objects.{self.subject}.linear_speed_m_s", + ), + subject_angular_speed=_nonnegative( + subject.get("angular_speed_rad_s"), + f"objects.{self.subject}.angular_speed_rad_s", + ), + target_position=target_position, + target_aabb_center_position=_apply_local_offset( + target_position, + target_rotation, + self.target_aabb_center_offset_m, + ), + eef_position=(eef_pose[0][3], eef_pose[1][3], eef_pose[2][3]), + eef_rotation=tuple(tuple(row[column] for column in range(3)) for row in eef_pose[:3]), + finger_aperture=aperture, + ) + + +def _validate_grasp_geometry(value: Any, subject_id: str) -> dict[str, Any]: + """Validate the finite, name-pinned grasp geometry for the off-center mesh.""" + + expected_scalars = { + "asset_name": _TASK_PROFILE.graspable_asset, + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "generator_storage": "reusable_finite_tuple", + "grasp_count": 4, + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": subject_id, + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "profile": _TASK_PROFILE.grasp_geometry_profile, + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + transform_keys = { + "converted_object_origin_from_aabb_center", + "link_from_object_transforms", + "primitive_link_from_aabb_center_transforms", + } + if not isinstance(value, Mapping) or set(value) != set(expected_scalars) | transform_keys: + raise ValueError("ScheduleStream plan has no exact grasp geometry") + if any(value.get(key) != expected for key, expected in expected_scalars.items()): + raise ValueError("ScheduleStream grasp geometry is incompatible") + object_from_aabb = _matrix4( + value.get("converted_object_origin_from_aabb_center"), + "grasp_geometry.converted_object_origin_from_aabb_center", + ) + primitive_values = value.get("primitive_link_from_aabb_center_transforms") + link_from_object_values = value.get("link_from_object_transforms") + if not isinstance(primitive_values, list) or not isinstance(link_from_object_values, list): + raise ValueError("ScheduleStream grasp transforms must be materialized lists") + if len(primitive_values) != 4 or len(link_from_object_values) != 4: + raise ValueError("ScheduleStream grasp geometry must contain exactly four transforms") + primitive = tuple( + _matrix4(item, f"grasp_geometry.primitive[{index}]") for index, item in enumerate(primitive_values) + ) + link_from_object = tuple( + _matrix4(item, f"grasp_geometry.link_from_object[{index}]") + for index, item in enumerate(link_from_object_values) + ) + if len(set(primitive)) != 4: + raise ValueError("ScheduleStream grasp primitives must be unique") + aabb_from_object = matrix4_inverse(object_from_aabb) + for index, (primitive_pose, link_from_object_pose) in enumerate(zip(primitive, link_from_object)): + expected = matrix4_multiply(primitive_pose, aabb_from_object) + if any( + not math.isclose(expected[row][column], link_from_object_pose[row][column], abs_tol=1e-7) + for row in range(4) + for column in range(4) + ): + raise ValueError(f"ScheduleStream grasp transform {index} violates its composition formula") + return {"geometry": dict(value), "object_from_aabb": object_from_aabb} + + +def _validate_placement_geometry(value: Any, subject_id: str, target_id: str) -> dict[str, Any]: + """Validate the exact name-pinned v1 placement geometry before execution.""" + + if not isinstance(value, Mapping): + raise ValueError("ScheduleStream plan has no destination placement geometry") + if ( + value.get("attested") is not True + or value.get("schema_version") != 1 + or value.get("profile") != _TASK_PROFILE.destination_placement_profile + or value.get("relation") != "on" + or value.get("general_inside_semantics") is not False + or value.get("frame_convention") != "parent_from_child_homogeneous_4x4" + or value.get("placement_model") != "destination_local_aabb_top_plane_shifted_downward" + ): + raise ValueError("ScheduleStream destination placement geometry is incompatible") + surface = value.get("surface_config") + if not isinstance(surface, Mapping): + raise ValueError("ScheduleStream destination placement has no SurfaceConfig geometry") + xy_extend = _finite(surface.get("xy_extend_m"), "destination_placement.surface_config.xy_extend_m") + z_offset = _finite(surface.get("z_offset_m"), "destination_placement.surface_config.z_offset_m") + sampled_extent = _vector3( + value.get("sampled_surface_extent_m"), + "destination_placement.sampled_surface_extent_m", + ) + if any(abs(component) > 1e-9 for component in sampled_extent): + raise ValueError("ScheduleStream destination placement must sample the exact bowl AABB center") + predicted_offset = _vector3( + value.get("predicted_aabb_center_offset_m"), + "destination_placement.predicted_aabb_center_offset_m", + ) + vertical_corridor = _nonnegative( + value.get("vertical_evidence_corridor_m"), + "destination_placement.vertical_evidence_corridor_m", + ) + if math.hypot(predicted_offset[0], predicted_offset[1]) > 1e-9 or abs(predicted_offset[2]) > vertical_corridor: + raise ValueError("ScheduleStream destination placement prediction exceeds its evidence corridor") + + offsets = {} + object_from_aabb = {} + dimensions_by_role = {} + expected_records = { + "subject": ( + subject_id, + _TASK_PROFILE.graspable_asset, + _TASK_PROFILE.graspable_aabb_dimension_bounds_m, + ), + "destination": ( + target_id, + _TASK_PROFILE.destination_asset, + _TASK_PROFILE.destination_aabb_dimension_bounds_m, + ), + } + for role, (object_id, asset_name, dimension_bounds) in expected_records.items(): + geometry = value.get(role) + if not isinstance(geometry, Mapping): + raise ValueError(f"ScheduleStream destination placement has no {role} geometry") + if ( + geometry.get("object_id") != object_id + or geometry.get("asset_name") != asset_name + or geometry.get("aabb_kind") != "converted_mesh_local_axis_aligned_bounding_box" + ): + raise ValueError(f"ScheduleStream destination placement {role} identity is incompatible") + dimensions = _vector3( + geometry.get("aabb_dimensions_m"), + f"destination_placement.{role}.aabb_dimensions_m", + ) + if any(not lower <= observed <= upper for observed, (lower, upper) in zip(dimensions, dimension_bounds)): + raise ValueError(f"ScheduleStream destination placement {role} dimensions are outside the reviewed profile") + offset = _vector3( + geometry.get("aabb_center_in_isaac_rigid_root_m"), + f"destination_placement.{role}.aabb_center_in_isaac_rigid_root_m", + ) + transform = _matrix4( + geometry.get("isaac_rigid_root_from_aabb_center"), + f"destination_placement.{role}.isaac_rigid_root_from_aabb_center", + ) + if any(not math.isclose(transform[index][3], offset[index], abs_tol=1e-8) for index in range(3)): + raise ValueError(f"ScheduleStream destination placement {role} center transform is inconsistent") + offsets[role] = offset + object_from_aabb[role] = _matrix4( + geometry.get("converted_object_origin_from_aabb_center"), + f"destination_placement.{role}.converted_object_origin_from_aabb_center", + ) + dimensions_by_role[role] = dimensions + + subject_dimensions = dimensions_by_role["subject"] + destination_dimensions = dimensions_by_role["destination"] + expected_xy_extend = -max(destination_dimensions[0], destination_dimensions[1]) + expected_z_offset = 0.03 - 0.5 * (destination_dimensions[2] + subject_dimensions[2]) + if not math.isclose(xy_extend, expected_xy_extend, abs_tol=1e-8) or not math.isclose( + z_offset, + expected_z_offset, + abs_tol=1e-8, + ): + raise ValueError("ScheduleStream destination placement SurfaceConfig is not derived from attested geometry") + expected_surface_extent = ( + max(0.0, destination_dimensions[0] + xy_extend), + max(0.0, destination_dimensions[1] + xy_extend), + 0.0, + ) + if any( + not math.isclose(observed, expected, abs_tol=1e-8) + for observed, expected in zip(sampled_extent, expected_surface_extent) + ): + raise ValueError("ScheduleStream destination placement sampled extent is inconsistent with SurfaceConfig") + if any( + not math.isclose(observed, expected, abs_tol=1e-8) for observed, expected in zip(predicted_offset, (0, 0, 0.03)) + ): + raise ValueError("ScheduleStream destination placement prediction is not the reviewed center offset") + return { + "geometry": dict(value), + "subject_object_from_aabb": object_from_aabb["subject"], + "subject_offset": offsets["subject"], + "target_offset": offsets["destination"], + } + + +def _apply_local_offset( + position: tuple[float, float, float], + rotation: tuple[tuple[float, float, float], ...], + offset: tuple[float, float, float], +) -> tuple[float, float, float]: + return tuple( + position[row] + sum(rotation[row][column] * offset[column] for column in range(3)) for row in range(3) + ) # type: ignore[return-value] + + +def _maximum_check(name: str, observed: float | int, threshold: float | int) -> dict[str, Any]: + """Return one bounded maximum check for the task-success report.""" + + return { + "name": name, + "observed": observed, + "operator": "<=", + "passed": observed <= threshold, + "threshold": threshold, + } + + +def _minimum_check(name: str, observed: float | int, threshold: float | int) -> dict[str, Any]: + """Return one bounded minimum check for the task-success report.""" + + return { + "name": name, + "observed": observed, + "operator": ">=", + "passed": observed >= threshold, + "threshold": threshold, + } + + +def _mapping(value: Any, field_name: str) -> Mapping[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"physical evidence {field_name} must be a mapping") + return value + + +def _finite(value: Any, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"physical evidence {field_name} must be numeric") + result = float(value) + if not math.isfinite(result): + raise ValueError(f"physical evidence {field_name} must be finite") + return result + + +def _nonnegative(value: Any, field_name: str) -> float: + result = _finite(value, field_name) + if result < 0: + raise ValueError(f"physical evidence {field_name} must be non-negative") + return result + + +def _vector3(value: Any, field_name: str) -> tuple[float, float, float]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)) or len(value) != 3: + raise ValueError(f"physical evidence {field_name} must be a 3-vector") + result = tuple(_finite(item, f"{field_name}[{index}]") for index, item in enumerate(value)) + return result # type: ignore[return-value] + + +def _matrix4(value: Any, field_name: str) -> tuple[tuple[float, float, float, float], ...]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)) or len(value) != 4: + raise ValueError(f"physical evidence {field_name} must be a 4x4 matrix") + rows = [] + for row_index, row in enumerate(value): + if not isinstance(row, Sequence) or isinstance(row, (str, bytes)) or len(row) != 4: + raise ValueError(f"physical evidence {field_name} must be a 4x4 matrix") + rows.append(tuple(_finite(item, f"{field_name}[{row_index}][]") for item in row)) + if any(abs(rows[3][index] - expected) > 1e-5 for index, expected in enumerate((0.0, 0.0, 0.0, 1.0))): + raise ValueError(f"physical evidence {field_name} must be a homogeneous transform") + rotation = tuple(tuple(rows[row][column] for column in range(3)) for row in range(3)) + for left in range(3): + for right in range(3): + dot = sum(rotation[index][left] * rotation[index][right] for index in range(3)) + expected = 1.0 if left == right else 0.0 + if abs(dot - expected) > 1e-4: + raise ValueError(f"physical evidence {field_name} rotation must be orthonormal") + determinant = ( + rotation[0][0] * (rotation[1][1] * rotation[2][2] - rotation[1][2] * rotation[2][1]) + - rotation[0][1] * (rotation[1][0] * rotation[2][2] - rotation[1][2] * rotation[2][0]) + + rotation[0][2] * (rotation[1][0] * rotation[2][1] - rotation[1][1] * rotation[2][0]) + ) + if abs(determinant - 1.0) > 1e-4: + raise ValueError(f"physical evidence {field_name} rotation determinant must be +1") + return tuple(rows) # type: ignore[return-value] + + +def _quaternion_rotation(value: Any, field_name: str) -> tuple[tuple[float, float, float], ...]: + if not isinstance(value, Sequence) or isinstance(value, (str, bytes)) or len(value) != 4: + raise ValueError(f"physical evidence {field_name} must be a quaternion") + x, y, z, w = (_finite(item, f"{field_name}[]") for item in value) + norm = math.sqrt(w * w + x * x + y * y + z * z) + if norm <= 1e-12: + raise ValueError(f"physical evidence {field_name} must have nonzero norm") + w, x, y, z = (item / norm for item in (w, x, y, z)) + return ( + (1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)), + (2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)), + (2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)), + ) + + +def _distance(first: Sequence[float], second: Sequence[float]) -> float: + return math.sqrt(sum((left - right) ** 2 for left, right in zip(first, second))) + + +def _relative_object_pose( + sample: _PhysicalSample, +) -> tuple[tuple[float, float, float], tuple[tuple[float, float, float], ...]]: + eef_transpose = tuple(zip(*sample.eef_rotation)) + delta = tuple(sample.subject_position[index] - sample.eef_position[index] for index in range(3)) + translation = tuple(sum(row[index] * delta[index] for index in range(3)) for row in eef_transpose) + rotation = tuple( + tuple( + sum(eef_transpose[row][index] * sample.subject_rotation[index][column] for index in range(3)) + for column in range(3) + ) + for row in range(3) + ) + return translation, rotation + + +def _rotation_distance( + first: tuple[tuple[float, float, float], ...], + second: tuple[tuple[float, float, float], ...], +) -> float: + trace = sum(first[index][axis] * second[index][axis] for index in range(3) for axis in range(3)) + cosine = min(1.0, max(-1.0, (trace - 1.0) / 2.0)) + return math.acos(cosine) diff --git a/isaac_autodata_interfaces/autonomous/profiles/__init__.py b/isaac_autodata_interfaces/autonomous/profiles/__init__.py new file mode 100644 index 0000000..b508ec0 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/profiles/__init__.py @@ -0,0 +1,18 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Concrete autonomous task profiles supported by the live runtime.""" + +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import ( + FRANKA_PICK_CUBE_INTO_BOWL, + AutonomousTaskProfile, + PickPlaceSuccessThresholds, +) + +__all__ = [ + "FRANKA_PICK_CUBE_INTO_BOWL", + "AutonomousTaskProfile", + "PickPlaceSuccessThresholds", +] diff --git a/isaac_autodata_interfaces/autonomous/profiles/franka_pick_cube_into_bowl.py b/isaac_autodata_interfaces/autonomous/profiles/franka_pick_cube_into_bowl.py new file mode 100644 index 0000000..ac9cdd7 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/profiles/franka_pick_cube_into_bowl.py @@ -0,0 +1,138 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Reviewed constants for the live Franka cube-into-bowl task.""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class PickPlaceSuccessThresholds: + """Physical thresholds for the reviewed cube-into-bowl success check. + + These limits are intentionally asset-specific. They are not general container semantics: + Arena's validated success term remains the semantic authority while the executor requires + independent physical evidence for grasp, transport, release, and stable placement. + """ + + attachment_distance_m: float = 0.05 + grasp_aperture_min_m: float = 0.040 + grasp_aperture_max_m: float = 0.075 + minimum_closed_samples: int = 10 + minimum_lift_m: float = 0.030 + minimum_transport_m: float = 0.050 + maximum_relative_translation_drift_m: float = 0.020 + maximum_relative_rotation_drift_rad: float = 0.35 + minimum_success_streak: int = 5 + maximum_final_linear_speed_m_s: float = 0.05 + maximum_final_angular_speed_rad_s: float = 0.5 + maximum_final_horizontal_radius_m: float = 0.028 + maximum_final_vertical_offset_m: float = 0.040 + maximum_destination_drift_m: float = 0.020 + minimum_final_eef_separation_m: float = 0.060 + minimum_final_aperture_m: float = 0.075 + + +@dataclass(frozen=True) +class AutonomousTaskProfile: + """Immutable facts shared by admission, planning, execution, and verification.""" + + name: str + motion_backend: str + schedulestream_application: str + schedulestream_plan_backend: str + embodiment_name: str + task_kind: str + success_relation: str + background_asset: str + graspable_asset: str + destination_asset: str + required_task_params: frozenset[str] + maximum_batch_size: int + maximum_successful_episodes: int + maximum_attempts_per_success: int + maximum_planner_time_s: float + interpolation_dt_s: float + command_to_observation_offset_m: tuple[float, float, float] + runtime_asset_profile: str + registry_usd_basename: str + runtime_usd_basename: str + runtime_usd_path: str + runtime_usd_bytes: int + runtime_usd_sha256: str + runtime_usd_max_bytes: int + franka_usd_basenames: frozenset[str] + ik_joint_limit_margin_rad: float + frame_max_position_error_m: float + frame_max_rotation_error_rad: float + grasp_max_position_roundoff_m: float + grasp_max_rotation_roundoff_rad: float + grasp_geometry_profile: str + destination_placement_profile: str + desired_aabb_center_vertical_offset_m: float + vertical_evidence_corridor_m: float + graspable_aabb_dimension_bounds_m: tuple[tuple[float, float], ...] + destination_aabb_dimension_bounds_m: tuple[tuple[float, float], ...] + finger_joint_names: tuple[str, str] + success_profile: str + success_thresholds: PickPlaceSuccessThresholds + arena_success_force_threshold_n: float + arena_success_velocity_threshold_m_s: float + + +FRANKA_PICK_CUBE_INTO_BOWL = AutonomousTaskProfile( + name="franka_pick_and_place_custream_v1", + motion_backend="curobo_v1", + schedulestream_application="custream", + schedulestream_plan_backend="schedulestream_custream", + embodiment_name="franka_ik", + task_kind="PickAndPlaceTask", + success_relation="on", + background_asset="maple_table_robolab", + graspable_asset="rubiks_cube_hot3d_robolab", + destination_asset="bowl_ycb_robolab", + required_task_params=frozenset({"background_scene", "destination_location", "pick_up_object"}), + maximum_batch_size=1_024, + maximum_successful_episodes=10, + maximum_attempts_per_success=5, + maximum_planner_time_s=60.0, + interpolation_dt_s=0.02, + command_to_observation_offset_m=(0.0, 0.0, -0.0036), + runtime_asset_profile="franka_ik_custream_v1_official_root_usd", + registry_usd_basename="franka_panda_hand_on_stand.usd", + runtime_usd_basename="panda_instanceable.usd", + runtime_usd_path=( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1/Isaac/IsaacLab/" + "Robots/FrankaEmika/panda_instanceable.usd" + ), + runtime_usd_bytes=8_038, + runtime_usd_sha256="7f5a0c0aa6760cfbd348e08bc464d4b94341f027f51c2d9e42406ceefcc7787f", + runtime_usd_max_bytes=1 << 20, + franka_usd_basenames=frozenset({"franka_panda_hand_on_stand.usd", "panda_instanceable.usd"}), + ik_joint_limit_margin_rad=1e-3, + frame_max_position_error_m=0.005, + frame_max_rotation_error_rad=0.01, + grasp_max_position_roundoff_m=1e-7, + grasp_max_rotation_roundoff_rad=1e-6, + grasp_geometry_profile="franka_rubiks_cube_offcenter_cuboid_top_v1", + destination_placement_profile="franka_rubiks_cube_to_ycb_bowl_aabb_top_plane_v1", + desired_aabb_center_vertical_offset_m=0.03, + vertical_evidence_corridor_m=0.04, + graspable_aabb_dimension_bounds_m=((0.05, 0.065),) * 3, + destination_aabb_dimension_bounds_m=((0.14, 0.17), (0.14, 0.17), (0.04, 0.07)), + finger_joint_names=("panda_finger_joint1", "panda_finger_joint2"), + success_profile="franka_rubiks_cube_into_ycb_bowl_v1", + success_thresholds=PickPlaceSuccessThresholds(), + arena_success_force_threshold_n=0.1, + arena_success_velocity_threshold_m_s=0.1, +) + +__all__ = [ + "FRANKA_PICK_CUBE_INTO_BOWL", + "AutonomousTaskProfile", + "PickPlaceSuccessThresholds", +] diff --git a/isaac_autodata_interfaces/autonomous/runtime_support.py b/isaac_autodata_interfaces/autonomous/runtime_support.py new file mode 100644 index 0000000..4592b20 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/runtime_support.py @@ -0,0 +1,898 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Import-free product capability gate for the live autonomous generation lane. + +Arena remains the authority that validates and links user-authored semantics. This module applies a +second, deliberately narrower check: whether a resolved graph can be executed by the concrete +AutoData runtime that is available today. It consumes only resolved plain data and selected runtime +names, so callers can run it before launching Isaac or importing a planner implementation. +""" + +from __future__ import annotations + +import hashlib +import re +from dataclasses import dataclass +from typing import Any + +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import FRANKA_PICK_CUBE_INTO_BOWL +from isaac_autodata_interfaces.autonomous.task_request_types import CompiledTaskRequest, canonical_json + +RUNTIME_SUPPORT_SCHEMA_VERSION = 1 +CURRENT_RUNTIME_SUPPORT = FRANKA_PICK_CUBE_INTO_BOWL.name + +_MAX_GRAPH_ID_LENGTH = 128 +_LIVE_GRAPH_ID_PATTERN = re.compile( + rf"[A-Za-z_][A-Za-z0-9_]{{0,{_MAX_GRAPH_ID_LENGTH - 1}}}", + flags=re.ASCII, +) + + +class RuntimeSupportError(AutonomousValidationError): + """A compiled task request is outside the currently executable autonomous product slice.""" + + +@dataclass(frozen=True) +class RuntimeSupportProfile: + """Attestable identity of one request admitted by the pure product capability gate.""" + + profile: str + request_digest: str + graph_digest: str + motion_backend: str + schedulestream_application: str + embodiment_node_id: str + embodiment_name: str + task_id: str + task_kind: str + success_state_spec_id: str + success_constraint_id: str + pick_up_object_id: str + destination_location_id: str + background_scene_id: str + schema_version: int = RUNTIME_SUPPORT_SCHEMA_VERSION + + def to_dict(self) -> dict[str, Any]: + """Return the complete deterministic profile attestation payload.""" + + return { + "embodiment": { + "name": self.embodiment_name, + "node_id": self.embodiment_node_id, + }, + "graph_digest": self.graph_digest, + "motion_backend": self.motion_backend, + "profile": self.profile, + "request_digest": self.request_digest, + "schedulestream_application": self.schedulestream_application, + "schema_version": self.schema_version, + "task": { + "background_scene_id": self.background_scene_id, + "destination_location_id": self.destination_location_id, + "id": self.task_id, + "kind": self.task_kind, + "pick_up_object_id": self.pick_up_object_id, + "success_constraint_id": self.success_constraint_id, + "success_state_spec_id": self.success_state_spec_id, + }, + } + + def canonical_json(self) -> str: + """Return canonical JSON suitable for parent/child process attestation.""" + + return canonical_json(self.to_dict()) + + @property + def digest(self) -> str: + """Return the SHA-256 digest of the complete runtime-support profile.""" + + return hashlib.sha256(self.canonical_json().encode("utf-8")).hexdigest() + + +def validate_runtime_support( + request: CompiledTaskRequest, + *, + motion_backend: str, + schedulestream_application: str, +) -> RuntimeSupportProfile: + """Validate and attest the currently executable source-free runtime slice. + + The first live product profile is intentionally narrow: one ``franka_ik`` embodiment, one + linked ``PickAndPlaceTask``, one parameter-free ``on`` success relation, one environment, and + the reviewed ``curobo_v1``/``custream`` pairing. Schema support for cuRobo v2 remains a design + target; it is not a claim that a live custream2 provider exists. + + Args: + request: Fully resolved, linked, source-demo-free request. + motion_backend: Motion backend selected by the import-free runtime probe. + schedulestream_application: ScheduleStream application selected by the compatibility gate. + + Returns: + Deterministic profile data that can be digest-attested across a process boundary. + + Raises: + RuntimeSupportError: If any selected runtime or linked-graph property is outside the + reviewed executable profile. + """ + + issues: list[ValidationIssue] = [] + _validate_selected_runtime(motion_backend, schedulestream_application, issues) + _validate_live_operational_limits(request, issues) + + num_envs = getattr(getattr(request, "generation", None), "num_envs", None) + if type(num_envs) is not int or num_envs != 1: + issues.append( + ValidationIssue( + ("generation", "num_envs"), + "live_num_envs_unsupported", + f"the current autonomous runtime requires exactly one environment; got {num_envs!r}", + ) + ) + + try: + linked_graph = request.linked_graph + except Exception: + linked_graph = None + if type(linked_graph) is not dict: + issues.append( + ValidationIssue( + ("arena", "linked_graph"), + "linked_graph_invalid", + "the resolved linked graph must be a plain mapping", + ) + ) + raise RuntimeSupportError(issues) + + nodes = _plain_mapping_list(linked_graph.get("nodes"), ("arena", "linked_graph", "nodes"), issues) + if len(nodes) != 4: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "nodes"), + "live_graph_node_count_unsupported", + f"the reviewed live profile requires exactly four scene nodes; got {len(nodes)}", + ) + ) + nodes_by_id = _validate_graph_nodes(nodes, issues) + embodiment = _select_embodiment(nodes, issues) + + tasks = _plain_mapping_list(linked_graph.get("tasks"), ("arena", "linked_graph", "tasks"), issues) + task = _select_task(tasks, issues) + + state_specs = _plain_mapping_list( + linked_graph.get("state_specs"), + ("arena", "linked_graph", "state_specs"), + issues, + ) + if len(state_specs) != 2: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "state_specs"), + "live_state_spec_count_unsupported", + f"the reviewed live profile requires exactly initial and success state specs; got {len(state_specs)}", + ) + ) + states_by_id = _validate_state_specs(state_specs, issues) + _validate_task_state_bindings(task, set(states_by_id), issues) + + try: + goal_stages = request.goal_stages + except Exception: + goal_stages = None + stage = _select_goal_stage(goal_stages, issues) + constraint = _select_goal_constraint(stage, issues) + + task_params = _validate_task(task, nodes_by_id, issues) + _validate_initial_state(task, task_params, states_by_id, issues) + _validate_stage_binding(task, stage, issues) + _validate_constraint(constraint, task_params, nodes_by_id, issues) + _validate_success_state_projection(task, constraint, states_by_id, issues) + + if issues: + raise RuntimeSupportError(issues) + + assert embodiment is not None + assert task is not None + assert stage is not None + assert constraint is not None + assert task_params is not None + return RuntimeSupportProfile( + profile=CURRENT_RUNTIME_SUPPORT, + request_digest=request.request_digest, + graph_digest=request.graph_digest, + motion_backend=motion_backend, + schedulestream_application=schedulestream_application, + embodiment_node_id=embodiment["id"], + embodiment_name=embodiment["name"], + task_id=task["id"], + task_kind=task["kind"], + success_state_spec_id=stage.success_state_spec_id, + success_constraint_id=constraint.id, + pick_up_object_id=task_params["pick_up_object"], + destination_location_id=task_params["destination_location"], + background_scene_id=task_params["background_scene"], + ) + + +def _validate_selected_runtime( + motion_backend: Any, + application: Any, + issues: list[ValidationIssue], +) -> None: + if motion_backend == "curobo_v2" and application == "custream2": + issues.append( + ValidationIssue( + ("runtime", "selected_motion_backend"), + "live_curobo_v2_unsupported", + "curobo_v2/custream2 is schema-compatible but has no reviewed live IsaacLab provider yet", + ) + ) + return + if motion_backend != FRANKA_PICK_CUBE_INTO_BOWL.motion_backend: + issues.append( + ValidationIssue( + ("runtime", "selected_motion_backend"), + "live_motion_backend_unsupported", + f"the current live profile requires {FRANKA_PICK_CUBE_INTO_BOWL.motion_backend!r}; " + f"got {motion_backend!r}", + ) + ) + if application != FRANKA_PICK_CUBE_INTO_BOWL.schedulestream_application: + issues.append( + ValidationIssue( + ("runtime", "schedulestream_application"), + "live_schedulestream_application_unsupported", + "the current live profile requires " + f"{FRANKA_PICK_CUBE_INTO_BOWL.schedulestream_application!r}; got {application!r}", + ) + ) + + +def _validate_live_operational_limits( + request: CompiledTaskRequest, + issues: list[ValidationIssue], +) -> None: + planner = getattr(request, "planner", None) + generation = getattr(request, "generation", None) + output = getattr(request, "output", None) + checks = ( + ( + getattr(planner, "collisions", None) is True, + ("planner", "collisions"), + "live_collisions_required", + "the reviewed live profile requires collision checking", + ), + ( + type(getattr(planner, "max_time_s", None)) in (int, float) + and 0 < float(planner.max_time_s) <= FRANKA_PICK_CUBE_INTO_BOWL.maximum_planner_time_s, + ("planner", "max_time_s"), + "live_planner_time_limit", + f"live planner max_time_s must be in (0, {FRANKA_PICK_CUBE_INTO_BOWL.maximum_planner_time_s:g}]", + ), + ( + type(getattr(planner, "batch_size", None)) is int + and 1 <= planner.batch_size <= FRANKA_PICK_CUBE_INTO_BOWL.maximum_batch_size, + ("planner", "batch_size"), + "live_batch_size_limit", + f"live planner batch_size must be in [1, {FRANKA_PICK_CUBE_INTO_BOWL.maximum_batch_size}]", + ), + ( + getattr(planner, "profile", None) is False, + ("planner", "profile"), + "live_profile_mode_unsupported", + "planner profiling is disabled in the reviewed live profile", + ), + ( + getattr(planner, "animate", None) is False, + ("planner", "animate"), + "live_animation_unsupported", + "planner animation is disabled in the reviewed live profile", + ), + ( + type(getattr(planner, "interpolation_dt_s", None)) in (int, float) + and abs(float(planner.interpolation_dt_s) - FRANKA_PICK_CUBE_INTO_BOWL.interpolation_dt_s) <= 1e-9, + ("planner", "interpolation_dt_s"), + "live_interpolation_dt_unsupported", + f"the reviewed live profile requires interpolation_dt_s={FRANKA_PICK_CUBE_INTO_BOWL.interpolation_dt_s:g}", + ), + ( + type(getattr(generation, "successful_episodes", None)) is int + and 1 <= generation.successful_episodes <= FRANKA_PICK_CUBE_INTO_BOWL.maximum_successful_episodes, + ("generation", "successful_episodes"), + "live_success_target_limit", + f"live successful_episodes must be in [1, {FRANKA_PICK_CUBE_INTO_BOWL.maximum_successful_episodes}]", + ), + ( + type(getattr(generation, "max_attempts", None)) is int + and type(getattr(generation, "successful_episodes", None)) is int + and generation.successful_episodes >= 1 + and generation.successful_episodes <= generation.max_attempts + and generation.max_attempts + <= generation.successful_episodes * FRANKA_PICK_CUBE_INTO_BOWL.maximum_attempts_per_success, + ("generation", "max_attempts"), + "live_attempt_limit", + "live max_attempts must be at least successful_episodes and at most five attempts per requested success", + ), + ( + getattr(output, "run_log", None) is not None, + ("output", "run_log"), + "live_run_log_required", + "the reviewed live profile requires a durable run_log JSONL ledger", + ), + ) + for accepted, path, code, message in checks: + if not accepted: + issues.append(ValidationIssue(path, code, message)) + + +def _plain_mapping_list( + value: Any, + path: tuple[str | int, ...], + issues: list[ValidationIssue], +) -> list[dict[str, Any]]: + if type(value) is not list: + issues.append(ValidationIssue(path, "linked_graph_shape_invalid", "expected a plain list")) + return [] + result: list[dict[str, Any]] = [] + for index, item in enumerate(value): + if type(item) is not dict: + issues.append( + ValidationIssue( + path + (index,), + "linked_graph_shape_invalid", + "expected a plain mapping", + ) + ) + # Preserve the source index so any additional count or identity failures continue to + # point at the original linked-graph location. + result.append({}) + continue + result.append(item) + return result + + +def _validate_graph_nodes( + nodes: list[dict[str, Any]], + issues: list[ValidationIssue], +) -> dict[str, dict[str, Any]]: + by_id: dict[str, dict[str, Any]] = {} + for index, node in enumerate(nodes): + path = ("arena", "linked_graph", "nodes", index) + node_id = node.get("id") + if not _is_graph_id(node_id): + issues.append(ValidationIssue(path + ("id",), "graph_id_invalid", _graph_id_message(node_id))) + continue + if node_id in by_id: + issues.append( + ValidationIssue( + path + ("id",), + "graph_id_duplicate", + f"graph node id {node_id!r} is duplicated", + ) + ) + continue + by_id[node_id] = node + return by_id + + +def _select_embodiment( + nodes: list[dict[str, Any]], + issues: list[ValidationIssue], +) -> dict[str, Any] | None: + indexed = [(index, node) for index, node in enumerate(nodes) if node.get("type") == "embodiment"] + if len(indexed) != 1: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "nodes"), + "live_embodiment_count_unsupported", + f"the current live profile requires exactly one embodiment node; got {len(indexed)}", + ) + ) + return None + index, embodiment = indexed[0] + name = embodiment.get("name") + if name != FRANKA_PICK_CUBE_INTO_BOWL.embodiment_name: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "nodes", index, "name"), + "live_embodiment_unsupported", + f"the current live profile requires {FRANKA_PICK_CUBE_INTO_BOWL.embodiment_name!r}; got {name!r}", + ) + ) + if embodiment.get("params") != {}: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "nodes", index, "params"), + "live_embodiment_params_unsupported", + "the reviewed Franka DIK frame/controller profile requires empty embodiment params", + ) + ) + return embodiment + + +def _select_task( + tasks: list[dict[str, Any]], + issues: list[ValidationIssue], +) -> dict[str, Any] | None: + if len(tasks) != 1: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "tasks"), + "live_task_count_unsupported", + f"the current live profile requires exactly one linked task; got {len(tasks)}", + ) + ) + return None + task = tasks[0] + if task.get("kind") != FRANKA_PICK_CUBE_INTO_BOWL.task_kind: + issues.append( + ValidationIssue( + ("arena", "linked_graph", "tasks", 0, "kind"), + "live_task_kind_unsupported", + f"the current live profile requires {FRANKA_PICK_CUBE_INTO_BOWL.task_kind!r}; got {task.get('kind')!r}", + ) + ) + return task + + +def _validate_state_specs( + states: list[dict[str, Any]], + issues: list[ValidationIssue], +) -> dict[str, tuple[int, dict[str, Any]]]: + by_id: dict[str, tuple[int, dict[str, Any]]] = {} + for index, state in enumerate(states): + path = ("arena", "linked_graph", "state_specs", index, "id") + state_id = state.get("id") + if not _is_graph_id(state_id): + issues.append(ValidationIssue(path, "graph_id_invalid", _graph_id_message(state_id))) + elif state_id in by_id: + issues.append(ValidationIssue(path, "graph_id_duplicate", f"state id {state_id!r} is duplicated")) + else: + by_id[state_id] = (index, state) + return by_id + + +def _validate_task_state_bindings( + task: dict[str, Any] | None, + state_ids: set[str], + issues: list[ValidationIssue], +) -> None: + if task is None: + return + for field_name in ("initial_state_spec_id", "success_state_spec_id"): + value = task.get(field_name) + path = ("arena", "linked_graph", "tasks", 0, field_name) + if not _is_graph_id(value): + issues.append(ValidationIssue(path, "graph_id_invalid", _graph_id_message(value))) + elif value not in state_ids: + issues.append( + ValidationIssue( + path, + "state_spec_reference_missing", + f"task references unknown state spec {value!r}", + ) + ) + + +def _select_goal_stage(value: Any, issues: list[ValidationIssue]) -> Any | None: + if type(value) is not tuple or len(value) != 1: + count = len(value) if isinstance(value, (list, tuple)) else "invalid" + issues.append( + ValidationIssue( + ("arena", "goal_stages"), + "live_goal_stage_count_unsupported", + f"the current live profile requires exactly one ordered goal stage; got {count}", + ) + ) + return None + stage = value[0] + if getattr(stage, "index", None) != 0: + issues.append( + ValidationIssue( + ("arena", "goal_stages", 0, "index"), + "goal_stage_index_invalid", + "the sole goal stage must have index 0", + ) + ) + return stage + + +def _select_goal_constraint(stage: Any | None, issues: list[ValidationIssue]) -> Any | None: + if stage is None: + return None + constraints = getattr(stage, "spatial_constraints", None) + if type(constraints) is not tuple or len(constraints) != 1: + count = len(constraints) if isinstance(constraints, (list, tuple)) else "invalid" + issues.append( + ValidationIssue( + ("arena", "goal_stages", 0, "spatial_constraints"), + "live_goal_constraint_count_unsupported", + f"the current live profile requires exactly one spatial success constraint; got {count}", + ) + ) + return None + return constraints[0] + + +def _validate_task( + task: dict[str, Any] | None, + nodes_by_id: dict[str, dict[str, Any]], + issues: list[ValidationIssue], +) -> dict[str, str] | None: + if task is None: + return None + task_path = ("arena", "linked_graph", "tasks", 0) + task_id = task.get("id") + if not _is_graph_id(task_id): + issues.append(ValidationIssue(task_path + ("id",), "graph_id_invalid", _graph_id_message(task_id))) + + params = task.get("params") + if type(params) is not dict or any(type(key) is not str for key in params): + issues.append( + ValidationIssue( + task_path + ("params",), + "task_params_invalid", + "PickAndPlaceTask params must be a plain mapping with string keys", + ) + ) + return None + actual_keys = frozenset(params) + for missing in sorted(FRANKA_PICK_CUBE_INTO_BOWL.required_task_params - actual_keys): + issues.append( + ValidationIssue( + task_path + ("params", missing), + "task_param_missing", + f"the current PickAndPlaceTask profile requires param {missing!r}", + ) + ) + for unsupported in sorted(actual_keys - FRANKA_PICK_CUBE_INTO_BOWL.required_task_params): + issues.append( + ValidationIssue( + task_path + ("params", unsupported), + "live_task_param_unsupported", + f"task param {unsupported!r} is outside the reviewed live profile", + ) + ) + + typed: dict[str, str] = {} + expected_types = { + "background_scene": "background", + "destination_location": "object", + "pick_up_object": "object", + } + expected_assets = { + "background_scene": FRANKA_PICK_CUBE_INTO_BOWL.background_asset, + "destination_location": FRANKA_PICK_CUBE_INTO_BOWL.destination_asset, + "pick_up_object": FRANKA_PICK_CUBE_INTO_BOWL.graspable_asset, + } + asset_issue_codes = { + "background_scene": "live_background_asset_unsupported", + "destination_location": "live_destination_asset_unsupported", + "pick_up_object": "live_pick_up_geometry_unsupported", + } + for name in sorted(FRANKA_PICK_CUBE_INTO_BOWL.required_task_params & actual_keys): + value = params[name] + path = task_path + ("params", name) + if not _is_graph_id(value): + issues.append(ValidationIssue(path, "graph_id_invalid", _graph_id_message(value))) + continue + typed[name] = value + node = nodes_by_id.get(value) + if node is None: + issues.append( + ValidationIssue(path, "task_param_node_missing", f"task param references unknown graph node {value!r}") + ) + elif node.get("type") != expected_types[name]: + issues.append( + ValidationIssue( + path, + "task_param_node_type_unsupported", + f"task param {name!r} requires a {expected_types[name]!r} node; {value!r} is {node.get('type')!r}", + ) + ) + elif node.get("name") != expected_assets[name] or node.get("params") != {}: + geometry_label = "cuboid top-grasp strategy" if name == "pick_up_object" else "reviewed live scene" + issues.append( + ValidationIssue( + path, + asset_issue_codes[name], + f"the {geometry_label} requires the unmodified {expected_assets[name]!r} asset; " + f"got name={node.get('name')!r}, " + f"params={node.get('params')!r}", + ) + ) + if typed.get("pick_up_object") == typed.get("destination_location") and "pick_up_object" in typed: + issues.append( + ValidationIssue( + task_path + ("params", "destination_location"), + "task_endpoints_not_distinct", + "pick_up_object and destination_location must reference distinct graph nodes", + ) + ) + return typed if set(typed) == FRANKA_PICK_CUBE_INTO_BOWL.required_task_params else None + + +def _validate_initial_state( + task: dict[str, Any] | None, + task_params: dict[str, str] | None, + states_by_id: dict[str, tuple[int, dict[str, Any]]], + issues: list[ValidationIssue], +) -> None: + """Require the exact reviewed, non-successful pick/place initial semantics.""" + + if task is None or task_params is None: + return + state_entry = states_by_id.get(task.get("initial_state_spec_id")) + if state_entry is None: + return + state_index, state = state_entry + path = ("arena", "linked_graph", "state_specs", state_index) + if state.get("is_delta") is not False: + issues.append( + ValidationIssue( + path + ("is_delta",), + "live_initial_state_delta_unsupported", + "the reviewed initial state must be a complete non-delta state", + ) + ) + if state.get("task_constraints") != []: + issues.append( + ValidationIssue( + path + ("task_constraints",), + "live_initial_task_constraints_unsupported", + "the reviewed initial state requires an empty task_constraints list", + ) + ) + constraints = state.get("spatial_constraints") + if type(constraints) is not list: + issues.append( + ValidationIssue( + path + ("spatial_constraints",), + "linked_graph_shape_invalid", + "the initial spatial constraints must be a plain list", + ) + ) + return + expected = { + ("is_anchor", task_params["background_scene"], None), + ("on", task_params["pick_up_object"], task_params["background_scene"]), + ("on", task_params["destination_location"], task_params["background_scene"]), + } + actual: list[tuple[str, str, str | None]] = [] + valid_shape = True + for index, constraint in enumerate(constraints): + constraint_path = path + ("spatial_constraints", index) + if type(constraint) is not dict: + issues.append( + ValidationIssue( + constraint_path, + "linked_graph_shape_invalid", + "each initial spatial constraint must be a plain mapping", + ) + ) + valid_shape = False + continue + if not _is_graph_id(constraint.get("id")): + issues.append( + ValidationIssue( + constraint_path + ("id",), + "graph_id_invalid", + _graph_id_message(constraint.get("id")), + ) + ) + valid_shape = False + if constraint.get("params") != {}: + issues.append( + ValidationIssue( + constraint_path + ("params",), + "live_initial_constraint_params_unsupported", + "reviewed initial spatial constraints require empty params", + ) + ) + valid_shape = False + kind = constraint.get("kind") + subject = constraint.get("subject") + reference = constraint.get("reference") + if ( + not isinstance(kind, str) + or not _is_graph_id(subject) + or (reference is not None and not _is_graph_id(reference)) + ): + valid_shape = False + continue + actual.append((kind, subject, reference)) + if not valid_shape or len(actual) != 3 or set(actual) != expected or len(set(actual)) != len(actual): + issues.append( + ValidationIssue( + path + ("spatial_constraints",), + "live_initial_state_semantics_unsupported", + "the reviewed initial state must anchor the Maple table and place the distinct cube and bowl on it", + ) + ) + + +def _validate_stage_binding( + task: dict[str, Any] | None, + stage: Any | None, + issues: list[ValidationIssue], +) -> None: + if task is None or stage is None: + return + comparisons = ( + ("task_id", task.get("id")), + ("task_kind", task.get("kind")), + ("success_state_spec_id", task.get("success_state_spec_id")), + ) + for field_name, expected in comparisons: + actual = getattr(stage, field_name, None) + if actual != expected: + issues.append( + ValidationIssue( + ("arena", "goal_stages", 0, field_name), + "goal_stage_task_mismatch", + f"goal stage {field_name} {actual!r} does not match linked task value {expected!r}", + ) + ) + + +def _validate_constraint( + constraint: Any | None, + task_params: dict[str, str] | None, + nodes_by_id: dict[str, dict[str, Any]], + issues: list[ValidationIssue], +) -> None: + if constraint is None: + return + path = ("arena", "goal_stages", 0, "spatial_constraints", 0) + constraint_id = getattr(constraint, "id", None) + if not _is_graph_id(constraint_id): + issues.append(ValidationIssue(path + ("id",), "graph_id_invalid", _graph_id_message(constraint_id))) + relation = getattr(constraint, "kind", None) + if relation != FRANKA_PICK_CUBE_INTO_BOWL.success_relation: + issues.append( + ValidationIssue( + path + ("kind",), + "live_goal_relation_unsupported", + "the current live profile requires relation " + f"{FRANKA_PICK_CUBE_INTO_BOWL.success_relation!r}; got {relation!r}", + ) + ) + subject = getattr(constraint, "subject", None) + reference = getattr(constraint, "reference", None) + for field_name, value in (("subject", subject), ("reference", reference)): + if not _is_graph_id(value): + issues.append(ValidationIssue(path + (field_name,), "graph_id_invalid", _graph_id_message(value))) + elif value not in nodes_by_id: + issues.append( + ValidationIssue( + path + (field_name,), + "goal_constraint_node_missing", + f"goal constraint references unknown graph node {value!r}", + ) + ) + try: + params = constraint.params + except Exception: + params = None + if type(params) is not dict: + issues.append( + ValidationIssue(path + ("params",), "goal_constraint_params_invalid", "constraint params are invalid") + ) + elif params: + issues.append( + ValidationIssue( + path + ("params",), + "live_goal_params_unsupported", + "the current live 'on' relation requires empty params", + ) + ) + if task_params is None: + return + expected = { + "subject": task_params["pick_up_object"], + "reference": task_params["destination_location"], + } + for field_name, actual in (("subject", subject), ("reference", reference)): + if actual != expected[field_name]: + issues.append( + ValidationIssue( + path + (field_name,), + "goal_constraint_task_mismatch", + f"goal {field_name} {actual!r} does not match task endpoint {expected[field_name]!r}", + ) + ) + + +def _validate_success_state_projection( + task: dict[str, Any] | None, + constraint: Any | None, + states_by_id: dict[str, tuple[int, dict[str, Any]]], + issues: list[ValidationIssue], +) -> None: + """Prove the typed goal constraint is exactly the linked success-state constraint.""" + + if task is None or constraint is None: + return + state_entry = states_by_id.get(task.get("success_state_spec_id")) + if state_entry is None: + return + state_index, state = state_entry + state_path = ("arena", "linked_graph", "state_specs", state_index) + if state.get("is_delta") is not True: + issues.append( + ValidationIssue( + state_path + ("is_delta",), + "live_success_state_delta_required", + "the reviewed success state must be a delta state", + ) + ) + if state.get("task_constraints") != []: + issues.append( + ValidationIssue( + state_path + ("task_constraints",), + "live_success_task_constraints_unsupported", + "the reviewed success state requires an empty task_constraints list", + ) + ) + constraints_path = ("arena", "linked_graph", "state_specs", state_index, "spatial_constraints") + raw_constraints = state.get("spatial_constraints") + if type(raw_constraints) is not list or len(raw_constraints) != 1: + count = len(raw_constraints) if type(raw_constraints) is list else "invalid" + issues.append( + ValidationIssue( + constraints_path, + "live_goal_constraint_count_unsupported", + f"the linked success state must contain exactly one spatial constraint; got {count}", + ) + ) + return + raw = raw_constraints[0] + if type(raw) is not dict: + issues.append( + ValidationIssue( + constraints_path + (0,), + "linked_graph_shape_invalid", + "the linked success constraint must be a plain mapping", + ) + ) + return + try: + typed_params = constraint.params + except Exception: + typed_params = None + expected = { + "id": getattr(constraint, "id", None), + "kind": getattr(constraint, "kind", None), + "params": typed_params, + "reference": getattr(constraint, "reference", None), + "subject": getattr(constraint, "subject", None), + } + actual = { + "id": raw.get("id"), + "kind": raw.get("kind"), + "params": raw.get("params", {}), + "reference": raw.get("reference"), + "subject": raw.get("subject"), + } + if actual != expected: + issues.append( + ValidationIssue( + ("arena", "goal_stages", 0, "spatial_constraints", 0), + "goal_projection_mismatch", + "the typed goal constraint does not exactly match the linked success-state constraint", + ) + ) + + +def _is_graph_id(value: Any) -> bool: + return type(value) is str and _LIVE_GRAPH_ID_PATTERN.fullmatch(value) is not None + + +def _graph_id_message(value: Any) -> str: + return ( + "live graph identity must be an ASCII USD-safe identifier matching " + f"[A-Za-z_][A-Za-z0-9_]{{0,{_MAX_GRAPH_ID_LENGTH - 1}}}; got {value!r}" + ) diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/__init__.py b/isaac_autodata_interfaces/autonomous/schedulestream/__init__.py new file mode 100644 index 0000000..f546917 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/__init__.py @@ -0,0 +1,60 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Import-safe public surface for the live ScheduleStream planner.""" + +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + MalformedScheduleStreamCommandError, + ScheduleStreamBoundaryError, + ScheduleStreamClosedError, + ScheduleStreamCommandError, + ScheduleStreamImportError, + ScheduleStreamLimitError, + ScheduleStreamLoweringContext, + ScheduleStreamLoweringLimits, + ScheduleStreamProviderError, + ScheduleStreamTimingError, +) +from isaac_autodata_interfaces.autonomous.schedulestream.custream_v1 import ( + V1_FRANKA_USD_BASENAMES, + V1IsaacLabCommandPlanner, + V1IsaacLabPlannerConfig, + build_v1_isaaclab_world, + create_v1_isaaclab_command_planner, +) +from isaac_autodata_interfaces.autonomous.schedulestream.episode_planner import ( + V1ScheduleStreamEpisodePlanner, + create_schedulestream_episode_planner, +) +from isaac_autodata_interfaces.autonomous.schedulestream.goal_lowering import ( + GoalCompilationError, + ScheduleStreamGoalSymbols, + compile_schedulestream_goal, + load_schedulestream_goal_symbols, +) + +__all__ = [ + "V1_FRANKA_USD_BASENAMES", + "GoalCompilationError", + "MalformedScheduleStreamCommandError", + "ScheduleStreamBoundaryError", + "ScheduleStreamClosedError", + "ScheduleStreamCommandError", + "ScheduleStreamGoalSymbols", + "ScheduleStreamImportError", + "ScheduleStreamLimitError", + "ScheduleStreamLoweringContext", + "ScheduleStreamLoweringLimits", + "ScheduleStreamProviderError", + "ScheduleStreamTimingError", + "V1IsaacLabCommandPlanner", + "V1IsaacLabPlannerConfig", + "V1ScheduleStreamEpisodePlanner", + "build_v1_isaaclab_world", + "compile_schedulestream_goal", + "create_schedulestream_episode_planner", + "create_v1_isaaclab_command_planner", + "load_schedulestream_goal_symbols", +] diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/command_types.py b/isaac_autodata_interfaces/autonomous/schedulestream/command_types.py new file mode 100644 index 0000000..79b58b4 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/command_types.py @@ -0,0 +1,233 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Pure contracts for the optional ScheduleStream compatibility boundary.""" + +from __future__ import annotations + +import math +from collections.abc import Callable, Mapping +from dataclasses import dataclass, field +from typing import Any + +from isaac_autodata_core.autonomous.task_motion import GoalPredicate, JsonValue + + +class ScheduleStreamBoundaryError(RuntimeError): + """Base class for safe, classified ScheduleStream boundary failures.""" + + code = "schedulestream_boundary_error" + + +class ScheduleStreamImportError(ScheduleStreamBoundaryError): + """Raised when a selected ScheduleStream application cannot be loaded.""" + + code = "schedulestream_import_error" + + +class ScheduleStreamClosedError(ScheduleStreamBoundaryError): + """Raised when a closed boundary or planner provider is reused.""" + + code = "schedulestream_closed" + + +class ScheduleStreamCommandError(ScheduleStreamBoundaryError): + """Base class for a malformed or unsupported native command.""" + + code = "schedulestream_command_error" + + def __init__(self, message: str, *, path: tuple[int, ...] = ()) -> None: + self.path = path + path_text = "$" + "".join(f"[{index}]" for index in path) + super().__init__(f"{path_text}: {message}") + + +class MalformedScheduleStreamCommandError(ScheduleStreamCommandError): + """Raised when a recognized native command violates its API contract.""" + + code = "malformed_schedulestream_command" + + +class UnsupportedScheduleStreamCommandError(ScheduleStreamCommandError): + """Raised when a native command has no faithful backend-neutral representation.""" + + code = "unsupported_schedulestream_command" + + +class ScheduleStreamLimitError(ScheduleStreamCommandError): + """Raised before an untrusted native stream exceeds a configured resource limit.""" + + code = "schedulestream_limit_exceeded" + + +class ScheduleStreamTimingError(ScheduleStreamCommandError): + """Raised when native command timing is invalid or disagrees with the executor tick.""" + + code = "schedulestream_timing_mismatch" + + +class ScheduleStreamProviderError(ScheduleStreamBoundaryError): + """Raised when a concrete runtime provider cannot create or run its planner.""" + + code = "schedulestream_provider_error" + + +@dataclass(frozen=True) +class ScheduleStreamCommandSymbols: + """Native command types and pose conversion for one ScheduleStream application. + + The concrete classes are injected so pure tests need neither ScheduleStream nor cuRobo. The + production symbol loader imports the selected application only when lowering begins. + """ + + application: str + commands_type: type + composite_type: type + configuration_type: type + trajectory_type: type + link_path_type: type + open_type: type + close_type: type + attach_type: type + detach_type: type + pose_to_matrix: Callable[[Any], Any] + + def __post_init__(self) -> None: + if self.application not in ("custream", "custream2"): + raise ValueError("application must be 'custream' or 'custream2'") + type_fields = ( + "commands_type", + "composite_type", + "configuration_type", + "trajectory_type", + "link_path_type", + "open_type", + "close_type", + "attach_type", + "detach_type", + ) + for field_name in type_fields: + if not isinstance(getattr(self, field_name), type): + raise TypeError(f"{field_name} must be a class") + if not callable(self.pose_to_matrix): + raise TypeError("pose_to_matrix must be callable") + + +@dataclass(frozen=True) +class ScheduleStreamLoweringLimits: + """Hard bounds applied before native arrays or recursive streams are materialized.""" + + max_command_nodes: int = 100_000 + max_nesting_depth: int = 32 + max_composite_width: int = 64 + max_samples_per_segment: int = 100_000 + max_total_samples: int = 1_000_000 + max_joints: int = 256 + max_duration_s: float = 86_400.0 + + def __post_init__(self) -> None: + integer_fields = ( + "max_command_nodes", + "max_nesting_depth", + "max_composite_width", + "max_samples_per_segment", + "max_total_samples", + "max_joints", + ) + for field_name in integer_fields: + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, int) or value < 1: + raise ValueError(f"{field_name} must be a positive integer") + if ( + isinstance(self.max_duration_s, bool) + or not isinstance(self.max_duration_s, (int, float)) + or not math.isfinite(float(self.max_duration_s)) + or self.max_duration_s <= 0 + ): + raise ValueError("max_duration_s must be a positive finite number") + object.__setattr__(self, "max_duration_s", float(self.max_duration_s)) + + +@dataclass(frozen=True) +class ScheduleStreamLoweringContext: + """Planner-neutral identity, timing, and name bindings for one native command stream.""" + + request_digest: str + snapshot_digest: str + seed: int + goal: tuple[GoalPredicate, ...] + eef_name: str + frame: str + step_dt_s: float + initial_gripper_value: float = 1.0 + eef_by_arm: Mapping[str, str] = field(default_factory=dict) + eef_by_link: Mapping[str, str] = field(default_factory=dict) + object_name_map: Mapping[str, str] = field(default_factory=dict) + initial_attachments_by_link: Mapping[str, str] = field(default_factory=dict) + attachment_verifier: str = "contact_and_relative_motion_v1" + backend_version: str | None = None + metadata: Mapping[str, JsonValue] = field(default_factory=dict) + + def __post_init__(self) -> None: + for field_name in ( + "request_digest", + "snapshot_digest", + "eef_name", + "frame", + "attachment_verifier", + ): + _require_text(getattr(self, field_name), field_name) + if isinstance(self.seed, bool) or not isinstance(self.seed, int) or self.seed < 0: + raise ValueError("seed must be a non-negative integer") + step_dt_s = _positive_finite(self.step_dt_s, "step_dt_s") + gripper = _finite(self.initial_gripper_value, "initial_gripper_value") + object.__setattr__(self, "step_dt_s", step_dt_s) + object.__setattr__(self, "initial_gripper_value", gripper) + object.__setattr__(self, "goal", tuple(self.goal)) + for predicate in self.goal: + if not isinstance(predicate, GoalPredicate): + raise TypeError("goal entries must be GoalPredicate instances") + for field_name in ( + "eef_by_arm", + "eef_by_link", + "object_name_map", + "initial_attachments_by_link", + ): + value = getattr(self, field_name) + normalized: dict[str, str] = {} + for key, item in sorted(value.items()): + _require_text(key, f"{field_name} key") + _require_text(item, f"{field_name}[{key}]") + normalized[key] = item + object.__setattr__(self, field_name, normalized) + if len(set(self.initial_attachments_by_link.values())) != len(self.initial_attachments_by_link): + raise ValueError("one initial object cannot be attached to multiple links") + if self.backend_version is not None: + _require_text(self.backend_version, "backend_version") + object.__setattr__(self, "metadata", dict(self.metadata)) + + +def _require_text(value: Any, field_name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{field_name} must be a non-empty string") + if len(value) > 512 or "\x00" in value: + raise ValueError(f"{field_name} must be at most 512 characters and contain no NUL") + return value + + +def _finite(value: Any, field_name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{field_name} must be a number") + result = float(value) + if not math.isfinite(result): + raise ValueError(f"{field_name} must be finite") + return result + + +def _positive_finite(value: Any, field_name: str) -> float: + result = _finite(value, field_name) + if result <= 0: + raise ValueError(f"{field_name} must be positive") + return result diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/custream_v1.py b/isaac_autodata_interfaces/autonomous/schedulestream/custream_v1.py new file mode 100644 index 0000000..bbcdf7b --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/custream_v1.py @@ -0,0 +1,1977 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Lazy semantic command-planner provider for ScheduleStream's cuRobo v1 IsaacLab bridge.""" + +from __future__ import annotations + +import importlib +import math +import os +from collections import Counter +from collections.abc import Callable, Iterable, Mapping +from dataclasses import dataclass +from types import MethodType +from typing import Any, Protocol + +from isaac_autodata_core.autonomous.dense_trace import ( + DenseAttachmentEvent, + DensePlanTrace, + task_motion_plan_from_dense_trace, +) +from isaac_autodata_core.autonomous.task_motion import ( + IDENTITY_MATRIX4, + GoalPredicate, + Matrix4, + TaskMotionPlan, + matrix4, + matrix4_error, + matrix4_inverse, + matrix4_multiply, +) +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import FRANKA_PICK_CUBE_INTO_BOWL +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + MalformedScheduleStreamCommandError, + ScheduleStreamClosedError, + ScheduleStreamImportError, + ScheduleStreamLimitError, + ScheduleStreamLoweringContext, + ScheduleStreamLoweringLimits, + ScheduleStreamProviderError, + ScheduleStreamTimingError, +) +from isaac_autodata_interfaces.autonomous.schedulestream.goal_lowering import ( + ScheduleStreamGoalSymbols, + compile_schedulestream_goal, + load_schedulestream_goal_symbols, +) + +_TASK_PROFILE = FRANKA_PICK_CUBE_INTO_BOWL + +# Public compatibility aliases. New integration code should consume the task profile directly. +V1_FRANKA_USD_BASENAMES = _TASK_PROFILE.franka_usd_basenames +V1_IK_JOINT_LIMIT_MARGIN_RAD = _TASK_PROFILE.ik_joint_limit_margin_rad +V1_FRAME_ATTESTATION_MAX_POSITION_ERROR_M = _TASK_PROFILE.frame_max_position_error_m +V1_FRAME_ATTESTATION_MAX_ROTATION_ERROR_RAD = _TASK_PROFILE.frame_max_rotation_error_rad +V1_GRASP_GEOMETRY_MAX_POSITION_ROUNDOFF_M = _TASK_PROFILE.grasp_max_position_roundoff_m +V1_GRASP_GEOMETRY_MAX_ROTATION_ROUNDOFF_RAD = _TASK_PROFILE.grasp_max_rotation_roundoff_rad +V1_REVIEWED_GRASPABLE_ASSET = _TASK_PROFILE.graspable_asset +V1_REVIEWED_DESTINATION_ASSET = _TASK_PROFILE.destination_asset +V1_DESTINATION_PLACEMENT_PROFILE = _TASK_PROFILE.destination_placement_profile +V1_GRASP_GEOMETRY_PROFILE = _TASK_PROFILE.grasp_geometry_profile +V1_DESIRED_AABB_CENTER_VERTICAL_OFFSET_M = _TASK_PROFILE.desired_aabb_center_vertical_offset_m +V1_VERTICAL_EVIDENCE_CORRIDOR_M = _TASK_PROFILE.vertical_evidence_corridor_m +V1_REVIEWED_GRASPABLE_AABB_DIMENSION_BOUNDS_M = _TASK_PROFILE.graspable_aabb_dimension_bounds_m +V1_REVIEWED_DESTINATION_AABB_DIMENSION_BOUNDS_M = _TASK_PROFILE.destination_aabb_dimension_bounds_m + +_ModuleLoader = Callable[[], Any] +_GoalSymbolsLoader = Callable[[str], ScheduleStreamGoalSymbols] +_WorldFactory = Callable[..., Any] +_WorldCloser = Callable[[Any], None] +_EefPoseReader = Callable[[int, str], Any] +_PrimitiveGraspGenerator = Callable[..., Iterable[Any]] + + +class _SeedSetter(Protocol): + """Keyword-only seed hook matching ``custream.utils.set_seed(**kwargs)``.""" + + def __call__(self, *, seed: int) -> None: + """Set the deterministic seed immediately before a planning attempt.""" + + ... + + +@dataclass(frozen=True) +class V1IsaacLabPlannerConfig: + """Bounded settings for the concrete cuRobo v1 IsaacLab command provider.""" + + batch_size: int = 10 + scale_dt: float = 5.0 + collisions: bool = True + max_time_s: float = 60.0 + profile: bool = False + animate: bool = False + video: bool = False + verbose: bool = False + visualize_spheres: bool = False + + def __post_init__(self) -> None: + if isinstance(self.batch_size, bool) or not isinstance(self.batch_size, int) or self.batch_size < 1: + raise ValueError("batch_size must be a positive integer") + if self.batch_size > 65_536: + raise ValueError("batch_size must not exceed 65536") + for field_name in ("scale_dt", "max_time_s"): + value = getattr(self, field_name) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{field_name} must be a number") + normalized = float(value) + if not math.isfinite(normalized) or normalized <= 0: + raise ValueError(f"{field_name} must be positive and finite") + object.__setattr__(self, field_name, normalized) + if self.max_time_s > 86_400: + raise ValueError("max_time_s must not exceed 86400") + for field_name in ( + "collisions", + "profile", + "animate", + "video", + "verbose", + "visualize_spheres", + ): + if type(getattr(self, field_name)) is not bool: + raise ValueError(f"{field_name} must be a boolean") + + +class V1IsaacLabCommandPlanner: + """Owned wrapper around a semantic subclass of upstream IsaacLab ``Planner``.""" + + application = _TASK_PROFILE.schedulestream_application + + def __init__( + self, + native_planner: Any, + *, + eef_pose_reader: _EefPoseReader | None = None, + seed_setter: _SeedSetter | None = None, + world_closer: _WorldCloser | None = None, + ) -> None: + self._native_planner = native_planner + self._eef_pose_reader = eef_pose_reader + self._seed_setter = seed_setter + self._world_closer = world_closer + self._closed = False + self._last_frame_evidence: dict[str, Any] = {} + + @property + def native_planner(self) -> Any: + """Return the native planner while it remains open.""" + + self._require_open() + return self._native_planner + + @property + def arm(self) -> str: + """ScheduleStream arm identifier used to compile the semantic goal.""" + + return self.native_planner.semantic_arm + + @property + def world(self) -> Any: + """Native custream world, exposed for explicit integration wiring.""" + + return self.native_planner.world + + @property + def planning_diagnostics(self) -> dict[str, Any]: + """Return bounded native planning diagnostics accumulated for the current world.""" + + diagnostics = getattr(self.world, "autodata_ik_joint_limit_evidence", None) + return dict(diagnostics) if isinstance(diagnostics, Mapping) else {} + + def plan_dense_trace( + self, + context: ScheduleStreamLoweringContext, + env_id: int = 0, + *, + link_name: str | None = None, + limits: ScheduleStreamLoweringLimits | None = None, + ) -> DensePlanTrace | None: + """Lower upstream ``PathController`` arrays after exact live body-offset attestation.""" + + self._require_open() + _require_env_id(env_id) + if context.frame != "world": + raise ScheduleStreamProviderError( + "upstream v1 PathController produces world-frame poses; lowering context.frame must be 'world'" + ) + if self._eef_pose_reader is None: + raise ScheduleStreamProviderError( + "dense v1 lowering requires an eef_pose_reader for cuRobo-to-Isaac frame attestation" + ) + if self._seed_setter is None: + raise ScheduleStreamProviderError("dense v1 lowering requires a custream seed_setter") + limits = limits or ScheduleStreamLoweringLimits() + observed_eef = _matrix_from_native(self._eef_pose_reader(env_id, context.eef_name), "observed_eef_pose") + try: + self._seed_setter(seed=context.seed) + except Exception as exc: + raise ScheduleStreamProviderError(f"failed to seed custream for attempt seed {context.seed}") from exc + controller = self._native_planner.plan_controller(env_id) + if controller is None: + return None + links = tuple(controller.link_poses) + if link_name is None: + matching = [link for link, eef in context.eef_by_link.items() if eef == context.eef_name and link in links] + if len(matching) == 1: + link_name = matching[0] + elif len(links) == 1: + link_name = links[0] + else: + raise ScheduleStreamProviderError( + f"cannot select one action link for EEF {context.eef_name!r}; controller links are {links}" + ) + if link_name not in controller.link_poses: + raise ScheduleStreamProviderError( + f"requested action link {link_name!r} is absent; controller links are {links}" + ) + eef_offset, frame_attestation = _attest_action_to_eef_transform( + self._native_planner, + action_link_name=link_name, + observed_eef=observed_eef, + ) + raw_link_rows = _rows_from_native( + controller.link_poses[link_name], + "controller.link_poses", + maximum_rows=limits.max_samples_per_segment, + expected_columns=7, + ) + if not raw_link_rows: + raise MalformedScheduleStreamCommandError("PathController contains no link poses") + raw_initial_body = _pose_vector_to_matrix(raw_link_rows[0], "controller.link_poses[0]") + initial_position_error, initial_rotation_error = matrix4_error( + raw_initial_body, + frame_attestation["current_action_link"], + ) + if initial_position_error > 0.005 or initial_rotation_error > 0.01: + raise ScheduleStreamProviderError( + "v1 controller initial action-link pose does not attest against the live planner " + f"state (position={initial_position_error:.6g}m, rotation={initial_rotation_error:.6g}rad)" + ) + poses = [] + raw_action_poses = [] + for index, row in enumerate(raw_link_rows): + raw_body = _pose_vector_to_matrix(row, f"controller.link_poses[{index}]") + raw_action_poses.append(raw_body) + lowered_eef = matrix4_multiply(raw_body, eef_offset) + poses.append(matrix4(lowered_eef, f"controller.link_poses[{index}]")) + + joint_names = _bounded_names(controller.joints, "controller.joints", limits.max_joints) + joint_positions = _rows_from_native( + controller.joint_positions, + "controller.joint_positions", + maximum_rows=limits.max_samples_per_segment, + expected_columns=len(joint_names), + ) + gripper_rows = _vector_from_native( + controller.gripper_actions, + "controller.gripper_actions", + maximum_values=limits.max_samples_per_segment, + ) + sample_count = len(poses) + if len(joint_positions) != sample_count or len(gripper_rows) != sample_count: + raise MalformedScheduleStreamCommandError( + "PathController poses, joint positions, and gripper actions must align one-to-one" + ) + evidence_indices = {0, sample_count - 1} + evidence_indices.update( + index for index in range(1, sample_count) if abs(gripper_rows[index] - gripper_rows[index - 1]) > 1e-6 + ) + self._last_frame_evidence = { + "samples": [ + { + "gripper": gripper_rows[index], + "index": index, + "lowered_eef": [list(row) for row in poses[index]], + "raw_action_link": [list(row) for row in raw_action_poses[index]], + } + for index in sorted(evidence_indices) + ], + "object_root_to_mesh": getattr(self._native_planner, "object_pose_offset_evidence", {}), + "ik_joint_limit_filter": self.planning_diagnostics, + "observed_initial_eef": [list(row) for row in observed_eef], + "planned_link_name": link_name, + "planner_reference_frame": getattr( + self._native_planner.world, + "autodata_reference_frame_evidence", + {}, + ), + "destination_placement": getattr( + self._native_planner, + "destination_placement_geometry", + {}, + ), + "grasp_geometry": getattr(self._native_planner, "grasp_geometry", {}), + "world_graspability": getattr(self._native_planner, "grasp_configuration", {}), + "static_frame_attestation": { + "action_to_observed_eef": [list(row) for row in frame_attestation["observed_offset"]], + "configured_action_offset": [list(row) for row in frame_attestation["configured_offset"]], + "configured_offset_position_error_m": frame_attestation["configured_position_error_m"], + "configured_offset_rotation_error_rad": frame_attestation["configured_rotation_error_rad"], + "initial_action_position_error_m": initial_position_error, + "initial_action_rotation_error_rad": initial_rotation_error, + }, + } + if sample_count > limits.max_total_samples: + raise ScheduleStreamLimitError( + f"PathController has {sample_count} samples; total limit is {limits.max_total_samples}" + ) + native_dt_s = _positive_float(self._native_planner.world.time_step, "world.time_step") + tolerance = max(1e-9, context.step_dt_s * 1e-6) + if abs(native_dt_s - context.step_dt_s) > tolerance: + raise ScheduleStreamTimingError( + f"v1 controller dt {native_dt_s:g}s does not match executor dt {context.step_dt_s:g}s" + ) + if sample_count * native_dt_s > limits.max_duration_s: + raise ScheduleStreamLimitError("PathController duration exceeds configured lowering limit") + attachment_events = _infer_attachment_events( + gripper_rows, + context.goal, + graspable_object=getattr(self._native_planner, "semantic_graspable_object", None), + ) + return DensePlanTrace( + eef_name=context.eef_name, + frame=context.frame, + poses=tuple(poses), + gripper_values=tuple(gripper_rows), + step_dt_s=native_dt_s, + gripper_settle_steps=max(1, math.ceil(0.2 / native_dt_s)), + joint_names=joint_names, + joint_positions=joint_positions, + attachment_events=attachment_events, + ) + + def plan_task_motion_plan( + self, + context: ScheduleStreamLoweringContext, + env_id: int = 0, + *, + link_name: str | None = None, + limits: ScheduleStreamLoweringLimits | None = None, + ) -> TaskMotionPlan | None: + """Plan and lower through the executable dense Cartesian controller representation.""" + + trace = self.plan_dense_trace(context, env_id, link_name=link_name, limits=limits) + if trace is None: + return None + metadata = dict(context.metadata) + metadata["schedulestream"] = { + "application": _TASK_PROFILE.schedulestream_application, + "attachment_event_source": "aligned_binary_gripper_transitions", + "attachment_events_preserved": True, + "destination_placement": getattr( + self._native_planner, + "destination_placement_geometry", + {}, + ), + "grasp_geometry": getattr(self._native_planner, "grasp_geometry", {}), + "frame_evidence": self._last_frame_evidence, + "frame_calibration": "configured_action_offset_attested_live", + "source": "isaaclab.PathController", + } + return task_motion_plan_from_dense_trace( + trace, + request_digest=context.request_digest, + snapshot_digest=context.snapshot_digest, + backend=_TASK_PROFILE.schedulestream_plan_backend, + backend_version=context.backend_version or "unknown", + seed=context.seed, + goal=context.goal, + metadata=metadata, + ) + + def close(self) -> None: + """Invoke an injected native closer, then drop all strong native references.""" + + if self._closed: + return + self._closed = True + planner = self._native_planner + self._native_planner = None + world = getattr(planner, "world", None) + try: + if world is not None and self._world_closer is not None: + self._world_closer(world) + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamProviderError(f"v1 world closer failed ({type(exc).__name__}: {message})") from exc + finally: + if planner is not None: + frames = getattr(planner, "frames", None) + if hasattr(frames, "clear"): + frames.clear() + planner.goal = None + planner.world = None + planner.env = None + + def __enter__(self) -> V1IsaacLabCommandPlanner: + self._require_open() + return self + + def __exit__(self, exc_type: Any, exc: Any, traceback: Any) -> None: + self.close() + + def _require_open(self) -> None: + if self._closed: + raise ScheduleStreamClosedError("v1 IsaacLab command planner is closed") + + +def _initialize_semantic_v1_planner( + planner: Any, + *, + env: Any, + module: Any, + predicates: tuple[GoalPredicate, ...], + graspable_object: str, + destination_object: str, + graspable_asset_name: str, + destination_asset_name: str, + arm: str | None, + config: V1IsaacLabPlannerConfig, + world_factory: _WorldFactory, + goal_symbols_loader: _GoalSymbolsLoader, + world_closer: _WorldCloser | None, +) -> None: + planner.env = env + planner.world = None + planner.collisions = config.collisions + planner.max_time = config.max_time_s + planner.animate = config.animate + planner.video = config.video + planner.verbose = config.verbose + planner.kwargs = {} + native_world = None + try: + _validate_goal_subjects_are_dynamic(planner.scene, predicates) + native_world = world_factory( + module, + planner.scene, + config, + graspable_object=graspable_object, + destination_object=destination_object, + graspable_asset_name=graspable_asset_name, + destination_asset_name=destination_asset_name, + ) + native_world.set_retract_conf() + native_world.initialize(batch_size=config.batch_size) + _install_v1_ik_joint_limit_filter(native_world) + planner.world = native_world + grasp_configuration = _validate_world_graspability(native_world, graspable_object) + grasp_geometry = grasp_configuration["grasp_geometry"] + object_pose_offsets, object_pose_offset_evidence = _capture_v1_object_pose_offsets(planner, module) + destination_placement_geometry = _validate_world_destination_placement( + native_world, + graspable_object=graspable_object, + destination_object=destination_object, + object_pose_offset_evidence=object_pose_offset_evidence, + ) + semantic_arm = _select_semantic_arm(tuple(native_world.arms), arm) + symbols = goal_symbols_loader(_TASK_PROFILE.schedulestream_application) + semantic_goal = compile_schedulestream_goal(predicates, symbols, arm=semantic_arm) + except Exception as exc: + planner.world = None + planner.env = None + _close_failed_staged_world(native_world, world_closer, exc) + raise + planner.world = native_world + planner.object_pose_offsets = object_pose_offsets + planner.object_pose_offset_evidence = object_pose_offset_evidence + planner.grasp_configuration = grasp_configuration + planner.grasp_geometry = grasp_geometry + planner.destination_placement_geometry = destination_placement_geometry + planner.semantic_graspable_object = graspable_object + planner.semantic_destination_object = destination_object + planner.semantic_arm = semantic_arm + planner.goal = semantic_goal + planner.frames = [] + planner.errors = Counter() + + +def _install_v1_ik_joint_limit_filter(world: Any) -> None: + """Reject v1 IK candidates that cuRobo motion generation would reject at its boundary. + + The pinned v1 stack can report an IK seed as successful even when a waypoint lies just outside + the robot model's position limits. ScheduleStream otherwise selects that seed and later fails + the whole symbolic skeleton during motion refinement. Filtering only those native candidates + preserves collision checking and lets the stream consider another already-computed IK seed. + """ + + original = getattr(world, "link_iterative_inverse_kinematics", None) + if not callable(original): + raise ScheduleStreamProviderError("v1 world has no callable link iterative IK method") + world.autodata_ik_joint_limit_evidence = { + "accepted_candidates": 0, + "calls": 0, + "candidate_solutions": 0, + "joint_limit_margin_rad": V1_IK_JOINT_LIMIT_MARGIN_RAD, + "policy": "all_waypoints_inside_native_position_limits", + "rejected_candidates": 0, + } + + def filtered_link_ik(_world: Any, *args: Any, **kwargs: Any) -> tuple[Any, Any]: + try: + joint_state, distances = original(*args, **kwargs) + return _filter_v1_ik_joint_limits(_world, joint_state, distances) + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError("failed to enforce v1 IK joint-limit compatibility") from exc + + world.link_iterative_inverse_kinematics = MethodType(filtered_link_ik, world) + + +def _filter_v1_ik_joint_limits(world: Any, joint_state: Any, distances: Any) -> tuple[Any, Any]: + """Retain only finite, in-limit IK candidates and record aggregate evidence. + + Native non-solutions may use non-finite distance sentinels. Normalize every sentinel to + positive infinity so NaN or negative infinity can never win downstream candidate selection. + """ + + import torch + + positions = getattr(joint_state, "position", None) + if not isinstance(positions, torch.Tensor) or positions.ndim != 4: + raise ScheduleStreamProviderError("v1 iterative IK positions must have shape [batch, seeds, steps, joints]") + if not isinstance(distances, torch.Tensor) or distances.shape != positions.shape[:2]: + raise ScheduleStreamProviderError("v1 iterative IK distances must align with batch and seed dimensions") + if distances.dtype not in (torch.float32, torch.float64): + raise ScheduleStreamProviderError("v1 iterative IK distances must use float32 or float64") + if not torch.isfinite(positions).all(): + raise ScheduleStreamProviderError("v1 iterative IK returned non-finite joint positions") + limit_distances = world.get_limit_distances(joint_state) + if not isinstance(limit_distances, torch.Tensor) or limit_distances.shape != positions.shape: + raise ScheduleStreamProviderError("v1 world returned malformed joint-limit distances") + if not torch.isfinite(limit_distances).all(): + raise ScheduleStreamProviderError("v1 world returned non-finite joint-limit distances") + + maximum_violation = torch.amax(limit_distances, dim=(-1, -2)) + native_candidates = torch.isfinite(distances) + inside_limits = maximum_violation <= -V1_IK_JOINT_LIMIT_MARGIN_RAD + rejected = native_candidates & ~inside_limits + accepted = native_candidates & inside_limits + filtered_distances = torch.where(accepted, distances, torch.full_like(distances, torch.inf)) + + diagnostics = getattr(world, "autodata_ik_joint_limit_evidence", None) + if not isinstance(diagnostics, dict): + raise ScheduleStreamProviderError("v1 IK joint-limit diagnostics were not initialized") + diagnostics["calls"] += 1 + diagnostics["candidate_solutions"] += int(native_candidates.sum().item()) + diagnostics["accepted_candidates"] += int(accepted.sum().item()) + diagnostics["rejected_candidates"] += int(rejected.sum().item()) + if bool(native_candidates.any().item()): + minimum_margin = float((-maximum_violation[native_candidates]).min().item()) + prior_margin = diagnostics.get("minimum_observed_margin_rad") + diagnostics["minimum_observed_margin_rad"] = ( + minimum_margin if prior_margin is None else min(float(prior_margin), minimum_margin) + ) + return joint_state, filtered_distances + + +def _infer_attachment_events( + gripper_values: tuple[float, ...], + goal: tuple[GoalPredicate, ...], + *, + graspable_object: Any, +) -> tuple[DenseAttachmentEvent, ...]: + """Recover reviewed symbolic attachment intent from an aligned binary pick/place trace. + + The upstream v1 ``PathController`` does not expose its symbolic Attach/Detach actions, but it + does preserve their binary gripper transitions at exact controller sample indices. The live + profile admits one task-selected movable object, so those transitions have an unambiguous + object binding. Anything other than one open-close-open place lifecycle (or open-close for a + terminal holding goal) is rejected instead of silently producing an unverifiable plan. + """ + + if ( + not isinstance(graspable_object, str) + or not graspable_object + or len(graspable_object) > 512 + or "\x00" in graspable_object + ): + raise MalformedScheduleStreamCommandError( + "v1 planner did not retain the task-selected graspable object for attachment lowering" + ) + if not goal: + raise MalformedScheduleStreamCommandError("v1 attachment lowering requires a non-empty semantic goal") + subjects = {predicate.subject for predicate in goal} + if subjects != {graspable_object}: + raise MalformedScheduleStreamCommandError( + "v1 attachment object does not exactly match the semantic goal subject" + ) + + states: list[str] = [] + for index, value in enumerate(gripper_values): + if value >= 1.0 - 1e-6: + states.append("open") + elif value <= -1.0 + 1e-6: + states.append("closed") + else: + raise MalformedScheduleStreamCommandError( + f"v1 binary-gripper trace has unsupported value {value:.6g} at sample {index}" + ) + transitions = [(0, states[0])] + transitions.extend((index, state) for index, state in enumerate(states[1:], start=1) if state != states[index - 1]) + relations = {predicate.relation.lower() for predicate in goal} + expected_states = ("open", "closed") if relations == {"holding"} else ("open", "closed", "open") + observed_states = tuple(state for _, state in transitions) + if observed_states != expected_states: + raise MalformedScheduleStreamCommandError( + "v1 binary-gripper lifecycle does not match the semantic goal; " + f"expected {expected_states}, got {observed_states}" + ) + + events = [ + DenseAttachmentEvent( + sample_index=transitions[1][0], + operation="attach", + object_name=graspable_object, + ) + ] + if len(transitions) == 3: + events.append( + DenseAttachmentEvent( + sample_index=transitions[2][0], + operation="detach", + object_name=graspable_object, + ) + ) + return tuple(events) + + +def _validate_world_graspability(world: Any, graspable_object: str) -> dict[str, Any]: + """Prove the constructed TAMP world exposes exactly the task-selected movable object.""" + + raw_names = getattr(world, "movable_names", None) + if isinstance(raw_names, str): + raise ScheduleStreamProviderError("v1 world movable_names must be a materialized name collection") + try: + movable_names = tuple(raw_names) + except TypeError as exc: + raise ScheduleStreamProviderError("v1 world has no materialized movable_names collection") from exc + if any(not isinstance(name, str) or not name or len(name) > 512 for name in movable_names): + raise ScheduleStreamProviderError("v1 world movable_names contains an invalid scene ID") + if movable_names != (graspable_object,): + raise ScheduleStreamProviderError( + "v1 world graspability does not match the task-selected pickup object; " + f"expected {(graspable_object,)}, got {movable_names}" + ) + return { + "grasp_geometry": _validate_world_grasp_geometry(world, graspable_object), + "task_selected_graspable_object": graspable_object, + "world_movable_names": list(movable_names), + } + + +def _validate_world_grasp_geometry(world: Any, graspable_object: str) -> dict[str, Any]: + """Validate the staged world's finite grasp transforms for the off-center mesh.""" + + raw = getattr(world, "autodata_grasp_geometry", None) + expected_static = { + "asset_name": V1_REVIEWED_GRASPABLE_ASSET, + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "generator_storage": "reusable_finite_tuple", + "grasp_count": 4, + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": graspable_object, + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "profile": V1_GRASP_GEOMETRY_PROFILE, + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + dynamic_keys = { + "converted_object_origin_from_aabb_center", + "link_from_object_transforms", + "primitive_link_from_aabb_center_transforms", + } + if not isinstance(raw, Mapping) or set(raw) != set(expected_static) | dynamic_keys: + raise ScheduleStreamProviderError("v1 world grasp geometry was not exactly validated") + if any(raw.get(key) != value for key, value in expected_static.items()): + raise ScheduleStreamProviderError("v1 world grasp geometry was not exactly validated") + primitive_values = raw.get("primitive_link_from_aabb_center_transforms") + link_from_object_values = raw.get("link_from_object_transforms") + if not isinstance(primitive_values, list) or not isinstance(link_from_object_values, list): + raise ScheduleStreamProviderError("v1 grasp transforms must be materialized lists") + if len(primitive_values) != 4 or len(link_from_object_values) != 4: + raise ScheduleStreamProviderError("v1 grasp geometry must contain exactly four transforms") + try: + object_from_aabb = _matrix_from_native( + raw["converted_object_origin_from_aabb_center"], + "grasp_geometry.converted_object_origin_from_aabb_center", + ) + aabb_from_object = matrix4_inverse(object_from_aabb) + primitive_transforms = tuple( + _matrix_from_native(value, f"grasp_geometry.primitive[{index}]") + for index, value in enumerate(primitive_values) + ) + link_from_object_transforms = tuple( + _matrix_from_native(value, f"grasp_geometry.link_from_object[{index}]") + for index, value in enumerate(link_from_object_values) + ) + except Exception as exc: + raise ScheduleStreamProviderError("v1 grasp geometry contains a malformed transform") from exc + if len(set(primitive_transforms)) != 4: + raise ScheduleStreamProviderError("v1 grasp geometry primitive transforms are not unique") + if len(set(link_from_object_transforms)) != 4: + raise ScheduleStreamProviderError("v1 grasp geometry transforms are not unique") + for index, (primitive, link_from_object) in enumerate(zip(primitive_transforms, link_from_object_transforms)): + expected = matrix4_multiply(primitive, aabb_from_object) + position_error, rotation_error = matrix4_error(expected, link_from_object) + if ( + position_error > V1_GRASP_GEOMETRY_MAX_POSITION_ROUNDOFF_M + or rotation_error > V1_GRASP_GEOMETRY_MAX_ROTATION_ROUNDOFF_RAD + ): + raise ScheduleStreamProviderError( + f"v1 grasp transform {index} does not match the validated composition formula " + f"(position={position_error:.6g}m, rotation={rotation_error:.6g}rad)" + ) + try: + world_object = world.get_object(graspable_object) + grasp_config = world_object.grasp_config + configured_poses = grasp_config.generator + except Exception as exc: + raise ScheduleStreamProviderError("v1 world has no inspectable grasp generator") from exc + if ( + getattr(grasp_config, "primitive", None) != "cuboid" + or getattr(grasp_config, "pitch_interval", None) != "top" + or not isinstance(configured_poses, tuple) + or len(configured_poses) != 4 + ): + raise ScheduleStreamProviderError("v1 world grasp generator config is incompatible") + configured_transforms = tuple( + _native_pose_matrix(pose, f"world_grasp_config.generator[{index}]") + for index, pose in enumerate(configured_poses) + ) + for index, (configured, link_from_object) in enumerate(zip(configured_transforms, link_from_object_transforms)): + position_error, rotation_error = matrix4_error(configured, link_from_object) + if ( + position_error > V1_GRASP_GEOMETRY_MAX_POSITION_ROUNDOFF_M + or rotation_error > V1_GRASP_GEOMETRY_MAX_ROTATION_ROUNDOFF_RAD + ): + raise ScheduleStreamProviderError( + f"v1 world grasp generator pose {index} does not match its geometry record " + f"(position={position_error:.6g}m, rotation={rotation_error:.6g}rad)" + ) + return dict(raw) + + +def _validate_world_destination_placement( + world: Any, + *, + graspable_object: str, + destination_object: str, + object_pose_offset_evidence: Mapping[str, Any], +) -> dict[str, Any]: + """Validate placement geometry and derive live rigid-root-to-AABB-center transforms.""" + + raw = getattr(world, "autodata_destination_placement_geometry", None) + if not isinstance(raw, Mapping) or raw.get("attested") is not True: + raise ScheduleStreamProviderError("v1 world destination placement profile was not attested") + if ( + raw.get("schema_version") != 1 + or raw.get("profile") != V1_DESTINATION_PLACEMENT_PROFILE + or raw.get("relation") != "on" + or raw.get("general_inside_semantics") is not False + ): + raise ScheduleStreamProviderError("v1 world destination placement geometry is incompatible") + result = dict(raw) + for role, expected_name in (("subject", graspable_object), ("destination", destination_object)): + raw_geometry = raw.get(role) + if not isinstance(raw_geometry, Mapping) or raw_geometry.get("object_id") != expected_name: + raise ScheduleStreamProviderError(f"v1 world destination placement {role} identity does not match") + if expected_name not in object_pose_offset_evidence: + raise ScheduleStreamProviderError(f"v1 world has no rigid-root offset evidence for {expected_name!r}") + try: + root_from_converted = _matrix_from_native( + object_pose_offset_evidence[expected_name], + f"object_root_to_converted[{expected_name}]", + ) + converted_from_aabb = _matrix_from_native( + raw_geometry["converted_object_origin_from_aabb_center"], + f"converted_from_aabb[{expected_name}]", + ) + root_from_aabb = matrix4_multiply(root_from_converted, converted_from_aabb) + except Exception as exc: + raise ScheduleStreamProviderError( + f"failed to derive rigid-root-to-AABB-center transform for {expected_name!r}" + ) from exc + geometry = dict(raw_geometry) + geometry["aabb_center_in_isaac_rigid_root_m"] = [root_from_aabb[index][3] for index in range(3)] + geometry["isaac_rigid_root_from_aabb_center"] = [list(row) for row in root_from_aabb] + result[role] = geometry + return result + + +def _select_semantic_arm(available_arms: tuple[str, ...], requested_arm: str | None) -> str: + if requested_arm is None: + if len(available_arms) != 1: + raise ScheduleStreamProviderError( + f"v1 semantic goal requires an explicit arm; world arms are {available_arms}" + ) + return available_arms[0] + if requested_arm not in available_arms: + raise ScheduleStreamProviderError( + f"requested arm {requested_arm!r} is not present; world arms are {available_arms}" + ) + return requested_arm + + +def _capture_v1_object_pose_offsets(planner: Any, module: Any) -> tuple[dict[str, Any], dict[str, Any]]: + """Capture immutable rigid-root-to-converted-mesh transforms before live synchronization.""" + + multiply_poses = getattr(module, "multiply_poses", None) + if not callable(multiply_poses): + raise ScheduleStreamProviderError("v1 planner module has no callable multiply_poses") + names = tuple(planner.scene.rigid_objects) + if len(names) > 10_000: + raise ScheduleStreamProviderError("IsaacLab scene has more than 10000 rigid objects") + offsets: dict[str, Any] = {} + evidence: dict[str, Any] = {} + for name in names: + try: + root_pose = planner.pose(name) + mesh_pose = planner.world.get_object_pose(name) + offset = multiply_poses(root_pose.inverse(), mesh_pose) + [offset_matrix] = offset.get_numpy_matrix() + matrix = _matrix_from_native(offset_matrix, f"object_root_to_mesh[{name}]") + except Exception as exc: + if isinstance(exc, ScheduleStreamProviderError): + raise + raise ScheduleStreamProviderError( + f"failed to capture fixed rigid-root-to-mesh transform for {name!r}" + ) from exc + offsets[name] = offset + evidence[name] = [list(row) for row in matrix] + return offsets, evidence + + +def _restore_v1_object_mesh_poses(planner: Any, module: Any, *, state: Any | None) -> None: + """Synchronize converted collision meshes without discarding their USD root offsets.""" + + offsets = getattr(planner, "object_pose_offsets", None) + if not isinstance(offsets, Mapping): + raise ScheduleStreamProviderError("v1 planner object pose offsets were not initialized") + scene_names = tuple(planner.scene.rigid_objects) + if set(offsets) != set(scene_names): + raise ScheduleStreamProviderError("v1 planner object pose offsets do not match the live rigid-object set") + multiply_poses = getattr(module, "multiply_poses", None) + if not callable(multiply_poses): + raise ScheduleStreamProviderError("v1 planner module has no callable multiply_poses") + for name in scene_names: + try: + root_pose = planner.pose(name, state) + planner.world.set_object_pose(name, multiply_poses(root_pose, offsets[name])) + except Exception as exc: + if isinstance(exc, ScheduleStreamProviderError): + raise + raise ScheduleStreamProviderError( + f"failed to synchronize converted mesh pose for rigid object {name!r}" + ) from exc + + +def _scene_state_with_curobo_pose_order(state: Any, name: str) -> dict[str, Any]: + """Shallow-copy one IsaacLab scene pose while converting XYZW to cuRobo v1 WXYZ. + + Args: + state: Live ``InteractiveScene.state`` mapping. + name: Articulation or rigid-object scene name whose root pose will be consumed by upstream + ScheduleStream. + + Returns: + A state mapping identical to ``state`` except for the selected root-pose quaternion order. + + Raises: + ScheduleStreamProviderError: If the one-environment live state is malformed. + """ + + import torch + + if not isinstance(state, Mapping): + raise ScheduleStreamProviderError("IsaacLab scene state must be a mapping") + if not isinstance(name, str) or not name: + raise ScheduleStreamProviderError("IsaacLab scene pose name must be a non-empty string") + for body_type, bodies in state.items(): + if not isinstance(bodies, Mapping) or name not in bodies: + continue + body_state = bodies[name] + if not isinstance(body_state, Mapping): + raise ScheduleStreamProviderError(f"IsaacLab state for {name!r} must be a mapping") + root_pose = body_state.get("root_pose") + if ( + not isinstance(root_pose, torch.Tensor) + or root_pose.ndim != 2 + or tuple(root_pose.shape) != (1, 7) + or root_pose.dtype not in (torch.float32, torch.float64) + or not bool(torch.isfinite(root_pose).all().item()) + ): + raise ScheduleStreamProviderError( + f"IsaacLab root pose for {name!r} must be one finite float32/float64 XYZ+XYZW row" + ) + root_pose_wxyz = torch.cat((root_pose[:, :3], root_pose[:, 6:7], root_pose[:, 3:6]), dim=1) + converted_body_state = dict(body_state) + converted_body_state["root_pose"] = root_pose_wxyz + converted_bodies = dict(bodies) + converted_bodies[name] = converted_body_state + converted_state = dict(state) + converted_state[body_type] = converted_bodies + return converted_state + raise ScheduleStreamProviderError(f"IsaacLab scene state has no root pose for {name!r}") + + +def _close_failed_staged_world( + world: Any | None, + world_closer: _WorldCloser | None, + initialization_error: Exception, +) -> None: + if world is None or world_closer is None: + return + try: + world_closer(world) + except Exception as cleanup_error: + initialization = ( + f"{type(initialization_error).__name__}: {str(initialization_error).replace(chr(10), ' ')[:300]}" + ) + cleanup = f"{type(cleanup_error).__name__}: {str(cleanup_error).replace(chr(10), ' ')[:300]}" + raise ScheduleStreamProviderError( + f"v1 planner initialization failed ({initialization}) and world cleanup failed ({cleanup})" + ) from cleanup_error + + +def _validate_v1_destination_request( + predicates: tuple[GoalPredicate, ...], + *, + graspable_object: str, + destination_object: str, + graspable_asset_name: str, + destination_asset_name: str, + arm: str | None, +) -> None: + """Fail closed unless the direct API describes the one reviewed placement capability.""" + + if not predicates or any(not isinstance(item, GoalPredicate) for item in predicates): + raise ValueError("goal must contain GoalPredicate instances") + for field_name, value in (("graspable_object", graspable_object), ("destination_object", destination_object)): + if not isinstance(value, str) or not value.strip() or len(value) > 512 or "\x00" in value: + raise ValueError(f"{field_name} must be a bounded non-empty scene ID") + if graspable_object == destination_object: + raise ValueError("graspable_object and destination_object must be distinct") + if graspable_asset_name != V1_REVIEWED_GRASPABLE_ASSET: + raise ValueError(f"graspable_asset_name must be the reviewed asset {V1_REVIEWED_GRASPABLE_ASSET!r}") + if destination_asset_name != V1_REVIEWED_DESTINATION_ASSET: + raise ValueError(f"destination_asset_name must be the reviewed asset {V1_REVIEWED_DESTINATION_ASSET!r}") + expected_goal = ("on", graspable_object, destination_object) + observed_goals = tuple((predicate.relation, predicate.subject, predicate.target) for predicate in predicates) + if observed_goals != (expected_goal,): + raise ValueError( + "the v1 destination placement profile requires exactly " + f"on({graspable_object}, {destination_object}); got {observed_goals}" + ) + if arm is not None and (not isinstance(arm, str) or not arm.strip() or "\x00" in arm): + raise ValueError("arm must be null or a non-empty string") + + +def create_v1_isaaclab_command_planner( + env: Any, + goal: Iterable[GoalPredicate], + *, + graspable_object: str, + destination_object: str, + graspable_asset_name: str, + destination_asset_name: str, + arm: str | None = None, + config: V1IsaacLabPlannerConfig | None = None, + module_loader: _ModuleLoader | None = None, + goal_symbols_loader: _GoalSymbolsLoader | None = None, + world_factory: _WorldFactory | None = None, + eef_pose_reader: _EefPoseReader | None = None, + seed_setter: _SeedSetter | None = None, + world_closer: _WorldCloser | None = None, +) -> V1IsaacLabCommandPlanner: + """Create the concrete semantic v1 provider without importing heavy modules beforehand. + + Args: + env: Live IsaacLab manager-based environment. + goal: Resolved backend-neutral goal predicates. + graspable_object: Exact live scene ID selected by the task as ``pick_up_object``. + destination_object: Exact live scene ID selected by the task as ``destination_location``. + graspable_asset_name: Linked Arena asset name for the pickup object. + destination_asset_name: Linked Arena asset name for the destination object. + arm: Optional custream arm ID; inferred only when the world has exactly one arm. + config: Bounded v1 runtime settings. + module_loader: Test hook returning the upstream IsaacLab planner module. + goal_symbols_loader: Test hook returning injected goal-language symbols. + world_factory: Test hook or alternate reviewed custream world construction function. + eef_pose_reader: Callback returning the live observed EEF 4x4 pose for frame attestation. + seed_setter: Test hook or custream RNG seeding callback invoked for every attempt. + world_closer: Optional explicit GPU/resource cleanup callback. Upstream v1 exposes no close. + """ + + if env is None: + raise ValueError("env must not be None") + predicates = tuple(goal) + _validate_v1_destination_request( + predicates, + graspable_object=graspable_object, + destination_object=destination_object, + graspable_asset_name=graspable_asset_name, + destination_asset_name=destination_asset_name, + arm=arm, + ) + config = config or V1IsaacLabPlannerConfig() + module_loader = module_loader or _load_v1_isaaclab_planner_module + goal_symbols_loader = goal_symbols_loader or load_schedulestream_goal_symbols + world_factory = world_factory or build_v1_isaaclab_world + seed_setter = seed_setter or _load_v1_seed_setter() + try: + module = module_loader() + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamImportError( + f"failed to import v1 IsaacLab planner ({type(exc).__name__}: {message})" + ) from exc + _validate_v1_module(module) + + class SemanticV1Planner(module.Planner): + """Upstream-compatible planner whose goal and world are supplied by AutoData.""" + + def __init__(self) -> None: + _initialize_semantic_v1_planner( + self, + env=env, + module=module, + predicates=predicates, + graspable_object=graspable_object, + destination_object=destination_object, + graspable_asset_name=graspable_asset_name, + destination_asset_name=destination_asset_name, + arm=arm, + config=config, + world_factory=world_factory, + goal_symbols_loader=goal_symbols_loader, + world_closer=world_closer, + ) + + def _pose(self, name: str, state: Any | None = None) -> Any: + """Bridge IsaacLab 6 XYZW scene state into pinned cuRobo v1 WXYZ poses.""" + + source_state = self.scene.state if state is None else state + converted_state = _scene_state_with_curobo_pose_order(source_state, name) + return super()._pose(name, state=converted_state) + + def set_env_state(self, env_id: int, state: Any | None = None, **kwargs: Any) -> None: + super().set_env_state(env_id, state=state, **kwargs) + _restore_v1_object_mesh_poses(self, module, state=state) + + def solve_commands(self, env_id: int) -> tuple[Any, Any | None]: + self.set_env_state(env_id) + state = self.world.state() + try: + with module.timeout_context(timeout=2 * self.max_time): + commands = module.solve_tamp( + state, + self.goal, + collisions=self.collisions, + max_time=self.max_time, + profile=config.profile, + ) + except Exception as exc: + failure_name = type(exc).__name__ + self.errors[failure_name] += 1 + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamProviderError( + f"v1 ScheduleStream planning failed ({failure_name}: {message})" + ) from exc + if self.animate or self.video: + self.frames.extend(module.animate_commands(state, commands, frequency=1, record=self.video)) + return state, commands + + def plan_controller(self, env_id: int) -> Any | None: + state, commands = self.solve_commands(env_id) + if commands is None: + state.set() + return None + try: + controller = module.create_controller(self, state, commands) + except Exception as exc: + raise ScheduleStreamProviderError("failed to construct aligned v1 PathController") from exc + finally: + state.set() + if controller is None: + raise ScheduleStreamProviderError("v1 create_controller returned None for a solved command stream") + return controller + + def current_link_matrix(self, link_name: str) -> Matrix4: + try: + pose = self.from_reference(self.world.get_node_pose(link_name)) + [native_matrix] = pose.get_numpy_matrix() + except Exception as exc: + raise ScheduleStreamProviderError( + f"failed to read current world pose for action link {link_name!r}" + ) from exc + return _matrix_from_native(native_matrix, f"current_link[{link_name}]") + + try: + native_planner = SemanticV1Planner() + except ScheduleStreamProviderError: + raise + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamProviderError( + f"failed to construct semantic v1 planner ({type(exc).__name__}: {message})" + ) from exc + return V1IsaacLabCommandPlanner( + native_planner, + eef_pose_reader=eef_pose_reader, + seed_setter=seed_setter, + world_closer=world_closer, + ) + + +def build_v1_isaaclab_world( + planner_module: Any, + scene: Any, + config: V1IsaacLabPlannerConfig, + *, + graspable_object: str, + destination_object: str, + graspable_asset_name: str, + destination_asset_name: str, + surface_config_factory: Callable[..., Any] | None = None, + primitive_grasp_generator: _PrimitiveGraspGenerator | None = None, +) -> Any: + """Construct the reviewed cube-to-bowl custream world and placement surrogate.""" + + articulations = tuple(scene.articulations) + if len(articulations) != 1: + raise ScheduleStreamProviderError( + f"v1 IsaacLab provider requires exactly one articulation, got {articulations}" + ) + robot = articulations[0] + articulation = scene.articulations[robot] + spawn = getattr(articulation.cfg, "spawn", None) + usd_path = getattr(spawn, "usd_path", None) + if not isinstance(usd_path, str) or not usd_path: + raise ScheduleStreamProviderError("robot articulation has no USD path") + usd_name = os.path.basename(usd_path) + if usd_name not in V1_FRANKA_USD_BASENAMES: + raise ScheduleStreamProviderError( + f"unsupported v1 robot USD {usd_name!r}; supported: {sorted(V1_FRANKA_USD_BASENAMES)}" + ) + objects = planner_module.create_objects(scene, env_id=0) + grasp_geometry = _repair_converted_rigid_object_mobility( + planner_module, + scene, + objects, + graspable_object=graspable_object, + graspable_asset_name=graspable_asset_name, + primitive_grasp_generator=primitive_grasp_generator, + ) + destination_placement_geometry = _configure_v1_destination_placement( + objects, + graspable_object=graspable_object, + destination_object=destination_object, + graspable_asset_name=graspable_asset_name, + destination_asset_name=destination_asset_name, + surface_config_factory=surface_config_factory or getattr(planner_module, "SurfaceConfig", None), + ) + robot_reference_pose = _live_robot_reference_pose(planner_module, scene, robot) + # Upstream object conversion uses the enclosing /Robot prim as its reference while Arena's + # stand USD places panda_link0 at a nontrivial transform beneath that prim. Rebase every + # converted obstacle into panda_link0 coordinates and keep cuRobo's robot base at identity. + # The inherited Planner.to_reference/from_reference methods then add the live articulation + # root exactly once when crossing between planner and world coordinates. + robot_config = planner_module.load_franka_config(base_poses=None) + sim_dt = float(scene.sim.get_physics_dt()) + if not math.isfinite(sim_dt) or sim_dt <= 0: + raise ScheduleStreamProviderError("IsaacLab physics dt must be positive and finite") + interpolation_dt = config.scale_dt * sim_dt + world = planner_module.World( + robot_config, + objects, + visualize_spheres=config.visualize_spheres, + interpolation_dt=interpolation_dt, + ) + world.autodata_destination_placement_geometry = destination_placement_geometry + world.autodata_grasp_geometry = grasp_geometry + try: + _rebase_v1_world_objects(world, planner_module, robot_reference_pose) + positions = scene.state["articulation"][robot]["joint_position"][0] + world.set_joint_positions(articulation.joint_names, positions) + world.set_camera_pose(planner_module.CAMERA_POSE) + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError("failed to synchronize initial IsaacLab robot state") from exc + return world + + +def _configure_v1_destination_placement( + converted_objects: Any, + *, + graspable_object: str, + destination_object: str, + graspable_asset_name: str, + destination_asset_name: str, + surface_config_factory: Callable[..., Any] | None, +) -> dict[str, Any]: + """Assign and attest the one reviewed v1 shifted-top-plane placement profile.""" + + if graspable_object == destination_object: + raise ScheduleStreamProviderError("graspable and destination objects must be distinct") + if graspable_asset_name != V1_REVIEWED_GRASPABLE_ASSET: + raise ScheduleStreamProviderError("v1 placement source is not the reviewed Rubik's cube asset") + if destination_asset_name != V1_REVIEWED_DESTINATION_ASSET: + raise ScheduleStreamProviderError("v1 placement destination is not the reviewed YCB bowl asset") + source = _unique_converted_object(converted_objects, graspable_object) + destination = _unique_converted_object(converted_objects, destination_object) + source_geometry = _converted_aabb_geometry(source, graspable_object) + destination_geometry = _converted_aabb_geometry(destination, destination_object) + _require_reviewed_aabb_dimension_bounds( + source_geometry["aabb_dimensions_m"], + V1_REVIEWED_GRASPABLE_AABB_DIMENSION_BOUNDS_M, + graspable_object, + ) + _require_reviewed_aabb_dimension_bounds( + destination_geometry["aabb_dimensions_m"], + V1_REVIEWED_DESTINATION_AABB_DIMENSION_BOUNDS_M, + destination_object, + ) + source_geometry["aabb_dimension_bounds_m"] = [ + list(bounds) for bounds in V1_REVIEWED_GRASPABLE_AABB_DIMENSION_BOUNDS_M + ] + destination_geometry["aabb_dimension_bounds_m"] = [ + list(bounds) for bounds in V1_REVIEWED_DESTINATION_AABB_DIMENSION_BOUNDS_M + ] + source_dimensions = source_geometry["aabb_dimensions_m"] + destination_dimensions = destination_geometry["aabb_dimensions_m"] + requested_xy_extend = -max(destination_dimensions[:2]) + requested_z_offset = V1_DESIRED_AABB_CENTER_VERTICAL_OFFSET_M - 0.5 * ( + destination_dimensions[2] + source_dimensions[2] + ) + + factory = surface_config_factory or _load_v1_surface_config_factory() + if not callable(factory): + raise ScheduleStreamProviderError("v1 SurfaceConfig factory is not callable") + try: + surface_config = factory( + xy_extend=requested_xy_extend, + z_offset=requested_z_offset, + ) + actual_xy_extend = _placement_float(surface_config.xy_extend, "SurfaceConfig.xy_extend") + actual_z_offset = _placement_float(surface_config.z_offset, "SurfaceConfig.z_offset") + surface_extend = _placement_vector3(surface_config.surface_extend, "SurfaceConfig.surface_extend") + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError("failed to construct the reviewed v1 SurfaceConfig") from exc + if not math.isclose(actual_xy_extend, requested_xy_extend, rel_tol=0.0, abs_tol=1e-12): + raise ScheduleStreamProviderError("v1 SurfaceConfig changed the reviewed xy_extend") + if not math.isclose(actual_z_offset, requested_z_offset, rel_tol=0.0, abs_tol=1e-12): + raise ScheduleStreamProviderError("v1 SurfaceConfig changed the reviewed z_offset") + expected_surface_extend = (actual_xy_extend, actual_xy_extend, 0.0) + if any( + not math.isclose(value, expected, rel_tol=0.0, abs_tol=1e-12) + for value, expected in zip(surface_extend, expected_surface_extend) + ): + raise ScheduleStreamProviderError("v1 SurfaceConfig.surface_extend has unexpected semantics") + + top_surface_dimensions = tuple(destination_dimensions[:2]) + (0.0,) + sampled_surface_extent = tuple( + max(0.0, dimension + extension) for dimension, extension in zip(top_surface_dimensions, surface_extend) + ) + if any(abs(value) > 1e-9 for value in sampled_surface_extent): + raise ScheduleStreamProviderError( + "reviewed v1 destination profile did not collapse the bowl AABB surface to its exact center" + ) + try: + for item in converted_objects: + item.surface_config = None + destination.surface_config = surface_config + except Exception as exc: + raise ScheduleStreamProviderError("failed to assign the reviewed destination SurfaceConfig") from exc + + source_height = source_dimensions[2] + destination_height = destination_dimensions[2] + predicted_vertical_offset = destination_height / 2.0 + actual_z_offset + source_height / 2.0 + if not math.isclose( + predicted_vertical_offset, + V1_DESIRED_AABB_CENTER_VERTICAL_OFFSET_M, + rel_tol=0.0, + abs_tol=1e-12, + ): + raise ScheduleStreamProviderError("reviewed placement derivation did not preserve its AABB-center target") + if abs(predicted_vertical_offset) > V1_VERTICAL_EVIDENCE_CORRIDOR_M: + raise ScheduleStreamProviderError("reviewed placement profile exceeds the physical-evidence vertical corridor") + return { + "attested": True, + "destination": { + "asset_name": destination_asset_name, + "object_id": destination_object, + **destination_geometry, + }, + "frame_convention": "parent_from_child_homogeneous_4x4", + "general_inside_semantics": False, + "limitations": [ + "name_pinned_local_aabb_geometry_not_container_interior_geometry", + "negative_top_plane_offset_is_a_gpu_validation_surrogate", + "pinned_v1_placement_collision_check_excludes_the_destination_parent", + ], + "placement_model": "destination_local_aabb_top_plane_shifted_downward", + "predicted_aabb_center_offset_m": [0.0, 0.0, predicted_vertical_offset], + "profile": V1_DESTINATION_PLACEMENT_PROFILE, + "relation": "on", + "sampled_surface_extent_m": list(sampled_surface_extent), + "schema_version": 1, + "subject": { + "asset_name": graspable_asset_name, + "object_id": graspable_object, + **source_geometry, + }, + "surface_config": { + "derivation": { + "desired_aabb_center_vertical_offset_m": V1_DESIRED_AABB_CENTER_VERTICAL_OFFSET_M, + "inputs": "attested_converted_local_aabb_dimensions_m", + "xy_extend_formula": "-max(destination_aabb_width_m,destination_aabb_depth_m)", + "z_offset_formula": "desired_center_z_m-0.5*(destination_aabb_height_m+subject_aabb_height_m)", + }, + "implementation": "schedulestream.applications.custream.object.SurfaceConfig", + "xy_extend_m": actual_xy_extend, + "z_offset_m": actual_z_offset, + }, + "vertical_evidence_corridor_m": V1_VERTICAL_EVIDENCE_CORRIDOR_M, + } + + +def _unique_converted_object(converted_objects: Any, name: str) -> Any: + if not isinstance(converted_objects, (list, tuple)): + raise ScheduleStreamProviderError("v1 converted objects must be a materialized list or tuple") + matches = [item for item in converted_objects if getattr(item, "name", None) == name] + if not matches: + raise ScheduleStreamProviderError(f"reviewed placement object {name!r} did not bind to a converted object") + if len(matches) != 1: + raise ScheduleStreamProviderError( + f"reviewed placement object {name!r} bound ambiguously to {len(matches)} converted objects" + ) + return matches[0] + + +def _converted_aabb_geometry(converted_object: Any, name: str) -> dict[str, Any]: + try: + bounding_box = converted_object.bounding_box + dimensions = _placement_vector3(bounding_box.dimensions, f"converted_aabb[{name}].dimensions") + [native_transform] = bounding_box.pose.get_numpy_matrix() + transform = _matrix_from_native(native_transform, f"converted_aabb[{name}].pose") + except Exception as exc: + raise ScheduleStreamProviderError(f"failed to attest converted local AABB for {name!r}") from exc + if any(value <= 0.0 for value in dimensions): + raise ScheduleStreamProviderError(f"converted local AABB for {name!r} must have positive dimensions") + return { + "aabb_center_in_converted_object_origin_m": [transform[index][3] for index in range(3)], + "aabb_dimensions_m": list(dimensions), + "aabb_kind": "converted_mesh_local_axis_aligned_bounding_box", + "converted_object_origin_from_aabb_center": [list(row) for row in transform], + } + + +def _require_reviewed_aabb_dimension_bounds( + actual: Any, + expected_bounds: tuple[tuple[float, float], tuple[float, float], tuple[float, float]], + name: str, +) -> None: + if any(not lower <= value <= upper for value, (lower, upper) in zip(actual, expected_bounds)): + raise ScheduleStreamProviderError( + f"converted local AABB for {name!r} is outside the reviewed per-axis bounds; " + f"expected {expected_bounds}, got {tuple(actual)}" + ) + + +def _placement_float(value: Any, field_name: str) -> float: + if isinstance(value, bool): + raise ScheduleStreamProviderError(f"{field_name} must be numeric") + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ScheduleStreamProviderError(f"{field_name} must be numeric") from exc + if not math.isfinite(result): + raise ScheduleStreamProviderError(f"{field_name} must be finite") + return result + + +def _placement_vector3(value: Any, field_name: str) -> tuple[float, float, float]: + materialized = _to_builtin(value, field_name) + if not isinstance(materialized, (list, tuple)) or len(materialized) != 3: + raise ScheduleStreamProviderError(f"{field_name} must contain three values") + return tuple(_placement_float(item, field_name) for item in materialized) # type: ignore[return-value] + + +def _live_robot_reference_pose(planner_module: Any, scene: Any, robot: str) -> Any: + """Return the enclosing robot-prim to live articulation-root transform. + + The reviewed profile has exactly one environment. Its finite world origin is mandatory because + silently assuming zero would move every collision object when the scene uses a translated origin. + IsaacLab's Warp-backed pose is ``XYZ + XYZW`` while cuRobo v1 consumes ``XYZ + WXYZ``; + the quaternion reorder is an explicit compatibility boundary. + """ + + converter = getattr(planner_module, "to_pose", None) + if not callable(converter): + raise ScheduleStreamProviderError("v1 planner module has no callable to_pose converter") + try: + root_pose_value = scene.state["articulation"][robot]["root_pose"] + [root_pose] = _rows_from_native( + root_pose_value, + f"scene.state.articulation[{robot}].root_pose", + maximum_rows=1, + expected_columns=7, + ) + env_origins = getattr(scene, "env_origins", None) + if env_origins is None: + raise ScheduleStreamProviderError("scene.env_origins is required for v1 reference-frame attestation") + [env_origin] = _rows_from_native( + env_origins, + "scene.env_origins", + maximum_rows=1, + expected_columns=3, + ) + relative_position = tuple(root_pose[index] - env_origin[index] for index in range(3)) + quaternion_wxyz = (root_pose[6], root_pose[3], root_pose[4], root_pose[5]) + pose = converter(relative_position + quaternion_wxyz) + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError(f"failed to derive live robot reference pose for {robot!r}") from exc + if pose is None: + raise ScheduleStreamProviderError("v1 to_pose converter returned null for the robot base") + return pose + + +def _rebase_v1_world_objects(world: Any, planner_module: Any, robot_reference_pose: Any) -> None: + """Move converted obstacles from the enclosing robot prim into articulation-root coordinates.""" + + multiply_poses = getattr(planner_module, "multiply_poses", None) + if not callable(multiply_poses): + raise ScheduleStreamProviderError("v1 planner module has no callable multiply_poses") + raw_names = getattr(world, "object_names", None) + if isinstance(raw_names, str): + raise ScheduleStreamProviderError("v1 world object_names must be a materialized name collection") + try: + object_names = tuple(raw_names) + except TypeError as exc: + raise ScheduleStreamProviderError("v1 world has no materialized object_names collection") from exc + if ( + not object_names + or len(object_names) > 100_000 + or len(set(object_names)) != len(object_names) + or any(not isinstance(name, str) or not name or len(name) > 1024 for name in object_names) + ): + raise ScheduleStreamProviderError("v1 world object_names is empty, duplicated, or malformed") + inverse = getattr(robot_reference_pose, "inverse", None) + if not callable(inverse): + raise ScheduleStreamProviderError("v1 robot reference pose has no callable inverse") + try: + reference_inverse = inverse() + [reference_matrix] = robot_reference_pose.get_numpy_matrix() + matrix = _matrix_from_native(reference_matrix, "robot_prim_to_articulation_root") + for name in object_names: + object_pose = world.get_object_pose(name) + world.set_object_pose(name, multiply_poses(reference_inverse, object_pose)) + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError("failed to rebase v1 obstacles into articulation-root coordinates") from exc + world.autodata_reference_frame_evidence = { + "converted_scene_frame": "enclosing_robot_prim", + "curobo_pose_quaternion_order": "wxyz", + "isaaclab_pose_quaternion_order": "xyzw", + "object_count": len(object_names), + "planner_frame": "articulation_root", + "robot_prim_to_articulation_root": [list(row) for row in matrix], + } + + +def _repair_converted_rigid_object_mobility( + planner_module: Any, + scene: Any, + converted_objects: Any, + *, + graspable_object: str, + graspable_asset_name: str, + primitive_grasp_generator: _PrimitiveGraspGenerator | None, +) -> dict[str, Any]: + """Make only the task-selected object graspable with the reviewed cube strategy.""" + + entries = _scene_rigid_object_entries(scene) + if not isinstance(converted_objects, (list, tuple)): + raise ScheduleStreamProviderError("v1 create_objects must return a materialized list or tuple") + if len(converted_objects) > 100_000: + raise ScheduleStreamProviderError("v1 create_objects returned more than 100000 objects") + grasp_config_type = getattr(planner_module, "GraspConfig", None) + if entries and not callable(grasp_config_type): + raise ScheduleStreamProviderError("v1 planner module has no callable GraspConfig") + entries_by_name = {name: is_kinematic for name, _, is_kinematic in entries} + if graspable_object not in entries_by_name: + raise ScheduleStreamProviderError( + f"task-selected graspable object {graspable_object!r} is absent from IsaacLab scene.rigid_objects" + ) + if entries_by_name[graspable_object]: + raise ScheduleStreamProviderError( + f"task-selected graspable object {graspable_object!r} is explicitly kinematic" + ) + + converted_by_name = {} + for scene_name, _, _ in entries: + matches = [item for item in converted_objects if getattr(item, "name", None) == scene_name] + if not matches: + raise ScheduleStreamProviderError( + f"Arena rigid object {scene_name!r} did not bind to any converted custream object" + ) + if len(matches) != 1: + raise ScheduleStreamProviderError( + f"Arena rigid object {scene_name!r} bound ambiguously to {len(matches)} converted custream objects" + ) + converted = matches[0] + converted_by_name[scene_name] = converted + try: + converted.grasp_config = None + except Exception as exc: + raise ScheduleStreamProviderError( + f"failed to classify converted custream object {scene_name!r} as non-graspable" + ) from exc + link_from_object_poses, grasp_geometry = _build_v1_analytical_grasps( + planner_module, + converted_by_name[graspable_object], + graspable_object=graspable_object, + graspable_asset_name=graspable_asset_name, + primitive_grasp_generator=primitive_grasp_generator, + ) + try: + converted_by_name[graspable_object].grasp_config = grasp_config_type( + primitive="cuboid", + pitch_interval="top", + generator=link_from_object_poses, + ) + except Exception as exc: + raise ScheduleStreamProviderError( + f"failed to install analytical grasp generator for {graspable_object!r}" + ) from exc + return grasp_geometry + + +def _build_v1_analytical_grasps( + planner_module: Any, + converted_object: Any, + *, + graspable_object: str, + graspable_asset_name: str, + primitive_grasp_generator: _PrimitiveGraspGenerator | None, +) -> tuple[tuple[Any, ...], dict[str, Any]]: + """Materialize the reviewed analytical top grasps in the object's true mesh frame.""" + + if graspable_asset_name != V1_REVIEWED_GRASPABLE_ASSET: + raise ScheduleStreamProviderError("v1 analytical grasp source is not the reviewed Rubik's cube asset") + geometry = _converted_aabb_geometry(converted_object, graspable_object) + _require_reviewed_aabb_dimension_bounds( + geometry["aabb_dimensions_m"], + V1_REVIEWED_GRASPABLE_AABB_DIMENSION_BOUNDS_M, + graspable_object, + ) + generator = primitive_grasp_generator or getattr(planner_module, "primitive_grasp_generator", None) + if generator is None: + generator = _load_v1_primitive_grasp_generator() + if not callable(generator): + raise ScheduleStreamProviderError("v1 primitive_grasp_generator is not callable") + multiply_poses = getattr(planner_module, "multiply_poses", None) + if not callable(multiply_poses): + raise ScheduleStreamProviderError("v1 planner module has no callable multiply_poses") + try: + raw_generator = iter(generator("cuboid", geometry["aabb_dimensions_m"], "top")) + primitive_poses = [] + for _ in range(5): + try: + primitive_poses.append(next(raw_generator)) + except StopIteration: + break + except Exception as exc: + raise ScheduleStreamProviderError("failed to materialize reviewed cuboid/top primitive grasps") from exc + if len(primitive_poses) != 4: + raise ScheduleStreamProviderError( + f"reviewed cuboid/top primitive grasp generator must yield exactly four poses, got {len(primitive_poses)}" + ) + try: + primitive_transforms = tuple( + _native_pose_matrix(pose, f"primitive_grasp[{index}]") for index, pose in enumerate(primitive_poses) + ) + if len(set(primitive_transforms)) != 4: + raise ScheduleStreamProviderError("reviewed cuboid/top primitive grasps must contain four unique poses") + bounding_box_from_object = converted_object.bounding_box.pose.inverse() + link_from_object_poses = tuple(multiply_poses(pose, bounding_box_from_object) for pose in primitive_poses) + link_from_object_transforms = tuple( + _native_pose_matrix(pose, f"grasp_geometry.link_from_object[{index}]") + for index, pose in enumerate(link_from_object_poses) + ) + except ScheduleStreamProviderError: + raise + except Exception as exc: + raise ScheduleStreamProviderError("failed to compose off-center cube grasp transforms") from exc + return link_from_object_poses, { + "asset_name": graspable_asset_name, + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "converted_object_origin_from_aabb_center": geometry["converted_object_origin_from_aabb_center"], + "link_from_object_transforms": [[list(row) for row in transform] for transform in link_from_object_transforms], + "generator_storage": "reusable_finite_tuple", + "grasp_count": len(link_from_object_poses), + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": graspable_object, + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "primitive_link_from_aabb_center_transforms": [ + [list(row) for row in transform] for transform in primitive_transforms + ], + "profile": V1_GRASP_GEOMETRY_PROFILE, + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + + +def _native_pose_matrix(pose: Any, field_name: str) -> Matrix4: + try: + [native_matrix] = pose.get_numpy_matrix() + return _matrix_from_native(native_matrix, field_name) + except Exception as exc: + raise ScheduleStreamProviderError(f"{field_name} must be one finite rigid pose") from exc + + +def _validate_goal_subjects_are_dynamic(scene: Any, predicates: tuple[GoalPredicate, ...]) -> None: + entries = {name: is_kinematic for name, _, is_kinematic in _scene_rigid_object_entries(scene)} + for predicate in predicates: + subject = predicate.subject + if subject not in entries: + raise ScheduleStreamProviderError( + f"goal subject {subject!r} does not bind to an IsaacLab scene.rigid_objects entry" + ) + if entries[subject]: + raise ScheduleStreamProviderError( + f"goal subject {subject!r} is explicitly kinematic and cannot be planned as a movable object" + ) + + +def _scene_rigid_object_entries(scene: Any) -> tuple[tuple[str, Any, bool], ...]: + rigid_objects = getattr(scene, "rigid_objects", None) + if not isinstance(rigid_objects, Mapping): + raise ScheduleStreamProviderError("IsaacLab scene.rigid_objects must be a mapping") + if len(rigid_objects) > 10_000: + raise ScheduleStreamProviderError("IsaacLab scene has more than 10000 rigid objects") + entries = [] + for name, rigid_object in rigid_objects.items(): + if not isinstance(name, str) or not name.strip() or len(name) > 512 or "\x00" in name: + raise ScheduleStreamProviderError("IsaacLab rigid-object names must be bounded non-empty strings") + cfg = getattr(rigid_object, "cfg", None) + spawn = None if cfg is None else getattr(cfg, "spawn", None) + if spawn is None: + raise ScheduleStreamProviderError(f"IsaacLab rigid object {name!r} has no reviewed spawn config") + rigid_props = getattr(spawn, "rigid_props", None) + kinematic = None if rigid_props is None else getattr(rigid_props, "kinematic_enabled", None) + if kinematic is not None and type(kinematic) is not bool: + raise ScheduleStreamProviderError( + f"IsaacLab rigid object {name!r} has a non-boolean kinematic_enabled value" + ) + entries.append((name, rigid_object, kinematic is True)) + return tuple(entries) + + +def _load_v1_isaaclab_planner_module() -> Any: + return importlib.import_module("schedulestream.applications.isaaclab.planner") + + +def _load_v1_surface_config_factory() -> Callable[..., Any]: + try: + module = importlib.import_module("schedulestream.applications.custream.object") + factory = module.SurfaceConfig + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamImportError( + f"failed to load custream SurfaceConfig ({type(exc).__name__}: {message})" + ) from exc + if not callable(factory): + raise ScheduleStreamImportError("custream SurfaceConfig symbol is not callable") + return factory + + +def _load_v1_primitive_grasp_generator() -> _PrimitiveGraspGenerator: + try: + module = importlib.import_module("schedulestream.applications.custream.grasp") + generator = module.primitive_grasp_generator + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamImportError( + f"failed to load custream primitive grasp generator ({type(exc).__name__}: {message})" + ) from exc + if not callable(generator): + raise ScheduleStreamImportError("custream primitive_grasp_generator symbol is not callable") + return generator + + +def _load_v1_seed_setter() -> _SeedSetter: + try: + module = importlib.import_module("schedulestream.applications.custream.utils") + setter = module.set_seed + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamImportError( + f"failed to load custream seed setter ({type(exc).__name__}: {message})" + ) from exc + if not callable(setter): + raise ScheduleStreamImportError("custream set_seed symbol is not callable") + return setter + + +def _validate_v1_module(module: Any) -> None: + required = ( + "CAMERA_POSE", + "Commands", + "GraspConfig", + "Planner", + "World", + "animate_commands", + "create_objects", + "create_controller", + "load_franka_config", + "multiply_poses", + "solve_tamp", + "timeout_context", + ) + missing = [name for name in required if not hasattr(module, name)] + if missing: + raise ScheduleStreamImportError(f"v1 IsaacLab planner module is missing symbols {missing}") + + +def _require_env_id(env_id: int) -> None: + if isinstance(env_id, bool) or not isinstance(env_id, int) or env_id < 0: + raise ValueError("env_id must be a non-negative integer") + + +def _action_body_offset(env: Any, link_name: str) -> Matrix4: + action_manager = getattr(env, "action_manager", None) + active_terms = () if action_manager is None else getattr(action_manager, "active_terms", ()) + matches = [] + for term_name in active_terms: + try: + term = action_manager.get_term(term_name) + cfg = term.cfg + except Exception as exc: + raise ScheduleStreamProviderError(f"failed to inspect IsaacLab action term {term_name!r}") from exc + if getattr(cfg, "body_name", None) == link_name: + matches.append(cfg) + if len(matches) != 1: + raise ScheduleStreamProviderError(f"expected one IK action term for body {link_name!r}, found {len(matches)}") + offset = getattr(matches[0], "body_offset", None) + if offset is None: + return IDENTITY_MATRIX4 + position = getattr(offset, "pos", (0.0, 0.0, 0.0)) + quaternion = getattr(offset, "rot", (0.0, 0.0, 0.0, 1.0)) + try: + position_values = tuple(position) + q_x, q_y, q_z, q_w = tuple(quaternion) + except TypeError as exc: + raise ScheduleStreamProviderError("IK action body_offset pos/rot must be sequences") from exc + except ValueError as exc: + raise ScheduleStreamProviderError("IK action body_offset rot must contain four XYZW values") from exc + vector = position_values + (q_w, q_x, q_y, q_z) + return _pose_vector_to_matrix(vector, f"action_term[{link_name}].body_offset") + + +def _attest_action_to_eef_transform( + native_planner: Any, + *, + action_link_name: str, + observed_eef: Matrix4, +) -> tuple[Matrix4, dict[str, Any]]: + """Require the live action-link-to-EEF transform to match the configured action offset.""" + + configured_offset = _action_body_offset(native_planner.base_env, action_link_name) + try: + current_action_link = native_planner.current_link_matrix(action_link_name) + except Exception as exc: + if isinstance(exc, ScheduleStreamProviderError): + raise + raise ScheduleStreamProviderError(f"failed to read live action-link pose {action_link_name!r}") from exc + _require_rigid_transform(current_action_link, f"current_action_link[{action_link_name}]") + _require_rigid_transform(observed_eef, "observed_eef_pose") + _require_rigid_transform(configured_offset, f"configured_action_offset[{action_link_name}]") + observed_offset = matrix4_multiply(matrix4_inverse(current_action_link), observed_eef) + _require_rigid_transform(observed_offset, f"action_to_observed_eef[{action_link_name}]") + configured_position_error, configured_rotation_error = matrix4_error( + configured_offset, + observed_offset, + ) + if ( + configured_position_error > V1_FRAME_ATTESTATION_MAX_POSITION_ERROR_M + or configured_rotation_error > V1_FRAME_ATTESTATION_MAX_ROTATION_ERROR_RAD + ): + observed_summary = tuple(tuple(round(value, 6) for value in row) for row in observed_offset) + raise ScheduleStreamProviderError( + "live action-to-EEF transform does not match the configured v1 action offset " + f"(position={configured_position_error:.6g}m, rotation={configured_rotation_error:.6g}rad, " + f"observed_action_to_eef={observed_summary})" + ) + return configured_offset, { + "configured_offset": configured_offset, + "configured_position_error_m": configured_position_error, + "configured_rotation_error_rad": configured_rotation_error, + "current_action_link": current_action_link, + "observed_offset": observed_offset, + } + + +def _matrix_from_native(value: Any, field_name: str) -> Matrix4: + materialized = _to_builtin(value, field_name) + if not isinstance(materialized, (list, tuple)) or len(materialized) != 4: + raise MalformedScheduleStreamCommandError(f"{field_name} must have shape [4, 4]") + rows = [] + for row in materialized: + if not isinstance(row, (list, tuple)) or len(row) != 4: + raise MalformedScheduleStreamCommandError(f"{field_name} must have shape [4, 4]") + rows.append(tuple(_finite_number(item, field_name) for item in row)) + try: + return matrix4(tuple(rows), field_name) + except ValueError as exc: + raise MalformedScheduleStreamCommandError(str(exc)) from exc + + +def _rows_from_native( + value: Any, + field_name: str, + *, + maximum_rows: int, + expected_columns: int, +) -> tuple[tuple[float, ...], ...]: + raw_shape = getattr(value, "shape", None) + if raw_shape is not None: + try: + shape = tuple(int(item) for item in raw_shape) + except (TypeError, ValueError, OverflowError) as exc: + raise MalformedScheduleStreamCommandError(f"{field_name} has an invalid shape") from exc + if len(shape) != 2 or not 1 <= shape[0] <= maximum_rows or shape[1] != expected_columns: + raise MalformedScheduleStreamCommandError( + f"{field_name} shape {shape} violates [1..{maximum_rows}, {expected_columns}]" + ) + materialized = _to_builtin(value, field_name) + if not isinstance(materialized, (list, tuple)) or not 1 <= len(materialized) <= maximum_rows: + raise MalformedScheduleStreamCommandError(f"{field_name} must contain between 1 and {maximum_rows} rows") + rows = [] + for row in materialized: + if not isinstance(row, (list, tuple)) or len(row) != expected_columns: + raise MalformedScheduleStreamCommandError(f"every {field_name} row must contain {expected_columns} values") + rows.append(tuple(_finite_number(item, field_name) for item in row)) + return tuple(rows) + + +def _vector_from_native( + value: Any, + field_name: str, + *, + maximum_values: int, +) -> tuple[float, ...]: + materialized = _to_builtin(value, field_name) + if not isinstance(materialized, (list, tuple)) or not 1 <= len(materialized) <= maximum_values: + raise MalformedScheduleStreamCommandError(f"{field_name} must contain between 1 and {maximum_values} values") + result = [] + for item in materialized: + if isinstance(item, (list, tuple)): + if len(item) != 1: + raise MalformedScheduleStreamCommandError(f"{field_name} must have shape [N] or [N, 1]") + item = item[0] + result.append(_finite_number(item, field_name)) + return tuple(result) + + +def _bounded_names(value: Any, field_name: str, maximum: int) -> tuple[str, ...]: + if not isinstance(value, (list, tuple)) or not 1 <= len(value) <= maximum: + raise MalformedScheduleStreamCommandError(f"{field_name} must contain between 1 and {maximum} names") + names = tuple(value) + if any(not isinstance(name, str) or not name.strip() or len(name) > 512 or "\x00" in name for name in names): + raise MalformedScheduleStreamCommandError(f"{field_name} entries must be bounded strings") + if len(names) != len(set(names)): + raise MalformedScheduleStreamCommandError(f"{field_name} entries must be unique") + return names + + +def _pose_vector_to_matrix(value: Any, field_name: str) -> Matrix4: + materialized = _to_builtin(value, field_name) + if not isinstance(materialized, (list, tuple)) or len(materialized) != 7: + raise MalformedScheduleStreamCommandError(f"{field_name} must contain [x, y, z, qw, qx, qy, qz]") + x_pos, y_pos, z_pos, q_w, q_x, q_y, q_z = (_finite_number(item, field_name) for item in materialized) + norm = math.sqrt(q_w * q_w + q_x * q_x + q_y * q_y + q_z * q_z) + if norm < 1e-12 or abs(norm - 1.0) > 1e-2: + raise MalformedScheduleStreamCommandError(f"{field_name} quaternion must have unit norm") + q_w, q_x, q_y, q_z = (item / norm for item in (q_w, q_x, q_y, q_z)) + result = ( + ( + 1.0 - 2.0 * (q_y * q_y + q_z * q_z), + 2.0 * (q_x * q_y - q_z * q_w), + 2.0 * (q_x * q_z + q_y * q_w), + x_pos, + ), + ( + 2.0 * (q_x * q_y + q_z * q_w), + 1.0 - 2.0 * (q_x * q_x + q_z * q_z), + 2.0 * (q_y * q_z - q_x * q_w), + y_pos, + ), + ( + 2.0 * (q_x * q_z - q_y * q_w), + 2.0 * (q_y * q_z + q_x * q_w), + 1.0 - 2.0 * (q_x * q_x + q_y * q_y), + z_pos, + ), + (0.0, 0.0, 0.0, 1.0), + ) + return matrix4(result, field_name) + + +def _require_rigid_transform(value: Matrix4, field_name: str, *, tolerance: float = 1e-3) -> None: + for first_column in range(3): + for second_column in range(3): + dot = sum(value[row][first_column] * value[row][second_column] for row in range(3)) + expected = 1.0 if first_column == second_column else 0.0 + if abs(dot - expected) > tolerance: + raise ScheduleStreamProviderError(f"{field_name} rotation is not orthonormal") + determinant = ( + value[0][0] * (value[1][1] * value[2][2] - value[1][2] * value[2][1]) + - value[0][1] * (value[1][0] * value[2][2] - value[1][2] * value[2][0]) + + value[0][2] * (value[1][0] * value[2][1] - value[1][1] * value[2][0]) + ) + if abs(determinant - 1.0) > tolerance: + raise ScheduleStreamProviderError(f"{field_name} rotation determinant is not +1") + + +def _to_builtin(value: Any, field_name: str) -> Any: + result = value + for method_name in ("detach", "cpu", "numpy", "tolist"): + method = getattr(result, method_name, None) + if callable(method): + try: + result = method() + except Exception as exc: + raise MalformedScheduleStreamCommandError( + f"failed to materialize {field_name} via {method_name}" + ) from exc + return result + + +def _finite_number(value: Any, field_name: str) -> float: + if isinstance(value, bool): + raise MalformedScheduleStreamCommandError(f"{field_name} must not contain booleans") + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise MalformedScheduleStreamCommandError(f"{field_name} must contain numbers") from exc + if not math.isfinite(result): + raise MalformedScheduleStreamCommandError(f"{field_name} must contain finite values") + return result + + +def _positive_float(value: Any, field_name: str) -> float: + result = _finite_number(value, field_name) + if result <= 0: + raise ScheduleStreamTimingError(f"{field_name} must be positive") + return result diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/episode_planner.py b/isaac_autodata_interfaces/autonomous/schedulestream/episode_planner.py new file mode 100644 index 0000000..ebf123c --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/episode_planner.py @@ -0,0 +1,388 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Narrow runtime factory exposing ScheduleStream as an autonomous ``EpisodePlanner``.""" + +from __future__ import annotations + +import json +import math +from collections.abc import Callable, Mapping +from typing import Any + +from isaac_autodata_core.autonomous.attempt_generation import AttemptGenerationError, AttemptRequest, FailureStage +from isaac_autodata_core.autonomous.task_motion import SceneSnapshot, TaskMotionPlan +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import FRANKA_PICK_CUBE_INTO_BOWL +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + ScheduleStreamClosedError, + ScheduleStreamLoweringContext, + ScheduleStreamProviderError, +) +from isaac_autodata_interfaces.autonomous.schedulestream.custream_v1 import ( + V1IsaacLabCommandPlanner, + V1IsaacLabPlannerConfig, + create_v1_isaaclab_command_planner, +) + +_V1Factory = Callable[..., V1IsaacLabCommandPlanner] +_TASK_PROFILE = FRANKA_PICK_CUBE_INTO_BOWL +_REVIEWED_REGISTRY_USD_SUFFIX = f"/Arena/assets/robot_library/{_TASK_PROFILE.registry_usd_basename}" +_MAX_REGISTRY_USD_PATH_LENGTH = 4_096 + + +class V1ScheduleStreamEpisodePlanner: + """IK-executable v1 planner using calibrated upstream ``PathController`` traces.""" + + def __init__( + self, + command_planner: V1IsaacLabCommandPlanner, + *, + eef_name: str, + link_name: str, + step_dt_s: float, + backend_version: str, + graph_digest: str, + planner_metadata: dict[str, Any], + runtime_asset_evidence: Mapping[str, Any], + success_contract_evidence: Mapping[str, Any], + ) -> None: + self._command_planner = command_planner + self._eef_name = eef_name + self._link_name = link_name + self._step_dt_s = step_dt_s + self._backend_version = backend_version + self._graph_digest = graph_digest + self._planner_metadata = planner_metadata + self._runtime_asset_evidence = dict(runtime_asset_evidence) + self._success_contract_evidence = dict(success_contract_evidence) + self._closed = False + + def plan(self, request: AttemptRequest, snapshot: SceneSnapshot) -> TaskMotionPlan: + """Plan one attempt and return only segments supported by the current IK executor.""" + + if self._closed: + raise ScheduleStreamClosedError("ScheduleStream episode planner is closed") + context = ScheduleStreamLoweringContext( + request_digest=request.request_digest, + snapshot_digest=snapshot.digest, + seed=request.seed, + goal=request.goal, + eef_name=self._eef_name, + frame="world", + step_dt_s=self._step_dt_s, + eef_by_link={self._link_name: self._eef_name}, + backend_version=self._backend_version, + metadata={ + "attempt_index": request.attempt_index, + "arena_success_contract": self._success_contract_evidence, + "graph_digest": self._graph_digest, + "planner": self._planner_metadata, + "runtime_asset": self._runtime_asset_evidence, + }, + ) + result = self._command_planner.plan_task_motion_plan( + context, + request.env_id, + link_name=self._link_name, + ) + if result is None: + diagnostics = self._command_planner.planning_diagnostics + diagnostic_suffix = "" + if diagnostics: + diagnostic_suffix = f"; diagnostics={json.dumps(diagnostics, sort_keys=True, separators=(',', ':'))}" + raise AttemptGenerationError( + FailureStage.PLANNING, + "schedulestream_no_plan", + f"ScheduleStream returned no plan within the configured limits{diagnostic_suffix}", + ) + return result + + def close(self) -> None: + """Close the owned native planner idempotently.""" + + if self._closed: + return + self._closed = True + self._command_planner.close() + + +def create_schedulestream_episode_planner( + bundle: Any, + resolved_request: Any, + compatibility: Any, + *, + attachment_state: Any | None = None, + v1_factory: _V1Factory | None = None, +) -> V1ScheduleStreamEpisodePlanner: + """Create the selected runtime planner after Isaac/Arena application launch. + + Args: + bundle: Live ``ArenaRuntimeBundle`` with environment and embodiment adapter. + resolved_request: Validated ``CompiledTaskRequest``. + compatibility: Result from ``select_schedulestream_backend``. + attachment_state: Shared executor state accepted for the stable integration seam. Dense v1 + lowering currently relies on gripper commands and final physical verification instead. + v1_factory: Pure-test injection hook for the concrete v1 planner factory. + """ + + del attachment_state + application = getattr(compatibility, "schedulestream_application", None) + motion_backend = getattr(compatibility, "motion_backend", None) + if application != _TASK_PROFILE.schedulestream_application or motion_backend != _TASK_PROFILE.motion_backend: + if application == "custream2" and motion_backend == "curobo_v2": + raise ScheduleStreamProviderError( + "custream2 command lowering is available, but no reviewed live v2 IsaacLab world/planner " + "factory is wired into the current image" + ) + raise ScheduleStreamProviderError( + f"unsupported selected ScheduleStream runtime application={application!r}, " + f"motion_backend={motion_backend!r}" + ) + if int(getattr(resolved_request.generation, "num_envs", 0)) != 1: + raise ScheduleStreamProviderError("the concrete ScheduleStream episode planner requires num_envs=1") + base_env = getattr(bundle.env, "unwrapped", bundle.env) + adapter = bundle.embodiment_adapter + eef_names = tuple(adapter.get_eef_names()) + if len(eef_names) != 1: + raise ScheduleStreamProviderError(f"v1 single-arm episode planner requires exactly one EEF, got {eef_names}") + eef_name = eef_names[0] + link_name = _single_action_body_name(base_env) + step_dt_s = _positive_finite(bundle.step_dt_s, "bundle.step_dt_s") + physics_dt_s = _positive_finite(base_env.sim.get_physics_dt(), "IsaacLab physics dt") + scale_dt = step_dt_s / physics_dt_s + requested_dt_s = _positive_finite( + resolved_request.planner.interpolation_dt_s, + "planner.interpolation_dt_s", + ) + if not math.isclose(requested_dt_s, step_dt_s, rel_tol=1e-5, abs_tol=1e-6): + raise ScheduleStreamProviderError( + f"planner dt {requested_dt_s:g}s does not match live environment dt {step_dt_s:g}s" + ) + config = V1IsaacLabPlannerConfig( + batch_size=resolved_request.planner.batch_size, + scale_dt=scale_dt, + collisions=resolved_request.planner.collisions, + max_time_s=resolved_request.planner.max_time_s, + profile=resolved_request.planner.profile, + animate=resolved_request.planner.animate, + ) + + from isaac_autodata_interfaces.autonomous.arena_environment import goal_predicates_from_request + + predicates = goal_predicates_from_request(resolved_request) + graspable_object, destination_object, graspable_asset_name, destination_asset_name = _task_pick_place_binding( + resolved_request, + predicates, + ) + success_contract_evidence = getattr(bundle, "success_contract_evidence", None) + if not isinstance(success_contract_evidence, Mapping) or success_contract_evidence.get("attested") is not True: + raise ScheduleStreamProviderError("live Arena success contract was not attested before planner construction") + runtime_asset_evidence = getattr(bundle, "runtime_asset_evidence", None) + _require_v1_runtime_asset_evidence(runtime_asset_evidence) + v1_factory = v1_factory or create_v1_isaaclab_command_planner + command_planner = v1_factory( + base_env, + predicates, + graspable_object=graspable_object, + destination_object=destination_object, + graspable_asset_name=graspable_asset_name, + destination_asset_name=destination_asset_name, + config=config, + eef_pose_reader=_world_eef_pose_reader(base_env, adapter), + ) + return V1ScheduleStreamEpisodePlanner( + command_planner, + eef_name=eef_name, + link_name=link_name, + step_dt_s=step_dt_s, + backend_version=_backend_version(compatibility), + graph_digest=resolved_request.graph_digest, + planner_metadata=resolved_request.planner.to_dict(), + runtime_asset_evidence=runtime_asset_evidence, + success_contract_evidence=success_contract_evidence, + ) + + +def _task_pick_place_binding(resolved_request: Any, predicates: tuple[Any, ...]) -> tuple[str, str, str, str]: + """Attest the linked cube-to-bowl task endpoints and exact semantic goal.""" + + linked_graph = getattr(resolved_request, "linked_graph", None) + tasks = linked_graph.get("tasks") if isinstance(linked_graph, dict) else None + if not isinstance(tasks, list) or len(tasks) != 1 or not isinstance(tasks[0], dict): + raise ScheduleStreamProviderError("resolved graph must contain exactly one linked task") + params = tasks[0].get("params") + pick_up_object = params.get("pick_up_object") if isinstance(params, dict) else None + destination_object = params.get("destination_location") if isinstance(params, dict) else None + if not _bounded_graph_id(pick_up_object): + raise ScheduleStreamProviderError("linked task has no valid pick_up_object scene ID") + if not _bounded_graph_id(destination_object): + raise ScheduleStreamProviderError("linked task has no valid destination_location scene ID") + if pick_up_object == destination_object: + raise ScheduleStreamProviderError("linked task pickup and destination IDs must be distinct") + observed_goals = tuple( + (getattr(predicate, "relation", None), getattr(predicate, "subject", None), getattr(predicate, "target", None)) + for predicate in predicates + ) + if observed_goals != (("on", pick_up_object, destination_object),): + raise ScheduleStreamProviderError("linked task pickup/destination does not exactly match one semantic on goal") + + nodes = linked_graph.get("nodes") + if not isinstance(nodes, list) or not nodes or any(not isinstance(node, dict) for node in nodes): + raise ScheduleStreamProviderError("resolved graph nodes must be a non-empty plain mapping list") + node_ids = [node.get("id") for node in nodes] + if any(not _bounded_graph_id(node_id) for node_id in node_ids) or len(node_ids) != len(set(node_ids)): + raise ScheduleStreamProviderError("resolved graph node IDs are invalid or duplicated") + nodes_by_id = {node["id"]: node for node in nodes} + expected = ( + (pick_up_object, _TASK_PROFILE.graspable_asset, "pickup"), + (destination_object, _TASK_PROFILE.destination_asset, "destination"), + ) + for object_id, asset_name, role in expected: + node = nodes_by_id.get(object_id) + if ( + not isinstance(node, dict) + or node.get("type") != "object" + or node.get("name") != asset_name + or node.get("params") != {} + ): + raise ScheduleStreamProviderError( + f"linked {role} {object_id!r} is not the unmodified reviewed asset {asset_name!r}" + ) + return ( + pick_up_object, + destination_object, + _TASK_PROFILE.graspable_asset, + _TASK_PROFILE.destination_asset, + ) + + +def _bounded_graph_id(value: Any) -> bool: + return isinstance(value, str) and bool(value.strip()) and len(value) <= 512 and "\x00" not in value + + +def _require_v1_runtime_asset_evidence(evidence: Any) -> None: + """Require the complete schema-v2 root-layer identity attestation without widening its scope.""" + + expected_root_layer = { + "attestation_method": "https_exact_url_sha256_v1", + "attested": True, + "bytes": _TASK_PROFILE.runtime_usd_bytes, + "content_encoding": "identity", + "expected_bytes": _TASK_PROFILE.runtime_usd_bytes, + "expected_sha256": _TASK_PROFILE.runtime_usd_sha256, + "final_url": _TASK_PROFILE.runtime_usd_path, + "http_status": 200, + "max_bytes": _TASK_PROFILE.runtime_usd_max_bytes, + "redirects_allowed": False, + "scope": "root_layer_bytes_only", + "sha256": _TASK_PROFILE.runtime_usd_sha256, + "url": _TASK_PROFILE.runtime_usd_path, + } + expected_top_level = { + "attested": True, + "attestation_scope": "runtime_usd_root_layer_identity_only", + "kinematic_frame_attestation": "separate_live_provider_attestation_required", + "motion_backend": _TASK_PROFILE.motion_backend, + "override": "composed_scene.robot.spawn.usd_path_only", + "profile": _TASK_PROFILE.runtime_asset_profile, + "reason": "pinned_official_isaac_5_1_root_layer_content_identity", + "referenced_usd_dependencies_attested": False, + "registry_usd_basename": _TASK_PROFILE.registry_usd_basename, + "runtime_usd_basename": _TASK_PROFILE.runtime_usd_basename, + "runtime_usd_path": _TASK_PROFILE.runtime_usd_path, + "runtime_usd_release": "Isaac 5.1", + "runtime_uri_policy": "pinned_exact_https_no_redirect", + "schedulestream_application": _TASK_PROFILE.schedulestream_application, + "schema_version": 2, + "semantic_embodiment": _TASK_PROFILE.embodiment_name, + } + if not isinstance(evidence, Mapping) or set(evidence) != set(expected_top_level) | { + "registry_usd_path", + "root_layer", + }: + raise ScheduleStreamProviderError("live custream v1 runtime asset schema-v2 keys were not exactly attested") + if any(evidence.get(key) != value for key, value in expected_top_level.items()): + raise ScheduleStreamProviderError("live custream v1 official-root runtime profile was not exactly attested") + registry_path = evidence.get("registry_usd_path") + if ( + not isinstance(registry_path, str) + or not registry_path + or registry_path != registry_path.strip() + or len(registry_path) > _MAX_REGISTRY_USD_PATH_LENGTH + or "\x00" in registry_path + or not registry_path.endswith(_REVIEWED_REGISTRY_USD_SUFFIX) + or registry_path.rsplit("/", 1)[-1] != evidence["registry_usd_basename"] + ): + raise ScheduleStreamProviderError( + "live Arena registry USD path is not a bounded path to the reviewed robot asset" + ) + root_layer = evidence.get("root_layer") + if not isinstance(root_layer, Mapping) or dict(root_layer) != expected_root_layer: + raise ScheduleStreamProviderError("live custream v1 runtime USD root layer was not exactly attested") + + +def _single_action_body_name(env: Any) -> str: + action_manager = getattr(env, "action_manager", None) + active_terms = () if action_manager is None else getattr(action_manager, "active_terms", ()) + body_names = [] + for term_name in active_terms: + try: + term = action_manager.get_term(term_name) + body_name = getattr(term.cfg, "body_name", None) + except Exception as exc: + raise ScheduleStreamProviderError(f"failed to inspect IsaacLab action term {term_name!r}") from exc + if isinstance(body_name, str) and body_name: + body_names.append(body_name) + body_names = list(dict.fromkeys(body_names)) + if len(body_names) != 1: + raise ScheduleStreamProviderError(f"expected exactly one Cartesian action body, got {body_names}") + return body_names[0] + + +def _world_eef_pose_reader(env: Any, adapter: Any) -> Callable[[int, str], Any]: + def read(env_id: int, eef_name: str) -> Any: + poses = adapter.get_eef_poses(env_ids=[env_id]) + if eef_name not in poses: + raise ScheduleStreamProviderError(f"embodiment adapter has no observed EEF {eef_name!r}") + pose = poses[eef_name][0] + clone = getattr(pose, "clone", None) + copy = getattr(pose, "copy", None) + result = clone() if callable(clone) else copy() if callable(copy) else pose + scene = getattr(env, "scene", None) + origins = None if scene is None else getattr(scene, "env_origins", None) + if origins is not None: + try: + result[:3, 3] += origins[env_id] + except Exception as exc: + raise ScheduleStreamProviderError( + "failed to convert observed EEF pose from environment origin to world frame" + ) from exc + return result + + return read + + +def _backend_version(compatibility: Any) -> str: + capabilities = getattr(compatibility, "capabilities", None) + identity = getattr(capabilities, "schedulestream", None) + version = getattr(identity, "version", None) + commit = getattr(identity, "source_commit", None) + if isinstance(version, str) and version: + if isinstance(commit, str) and commit: + return f"{version}+{commit[:12]}" + return version + return "unknown" + + +def _positive_finite(value: Any, field_name: str) -> float: + if isinstance(value, bool): + raise ScheduleStreamProviderError(f"{field_name} must be numeric") + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ScheduleStreamProviderError(f"{field_name} must be numeric") from exc + if not math.isfinite(result) or result <= 0: + raise ScheduleStreamProviderError(f"{field_name} must be positive and finite") + return result diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/experimental/__init__.py b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/__init__.py new file mode 100644 index 0000000..a52666d --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Experimental native-command lowering reserved for a future live backend.""" + +from isaac_autodata_interfaces.autonomous.schedulestream.experimental.command_lowering import ( + ScheduleStreamCommandLowerer, +) +from isaac_autodata_interfaces.autonomous.schedulestream.experimental.symbols import load_schedulestream_command_symbols + +__all__ = [ + "ScheduleStreamCommandLowerer", + "load_schedulestream_command_symbols", +] diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/experimental/command_lowering.py b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/command_lowering.py new file mode 100644 index 0000000..d9525c5 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/command_lowering.py @@ -0,0 +1,1199 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Experimentally lower native ScheduleStream commands into task-motion plans. + +This module is not used by the live cuStream v1 product path. It remains isolated for evaluation +while a future backend proves which native command forms the executor actually needs. +""" + +from __future__ import annotations + +import math +from collections.abc import Callable, Sequence +from dataclasses import dataclass, field +from typing import Any + +from isaac_autodata_core.autonomous.dense_trace import DensePlanTrace +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + ConcurrentGroupSegment, + DetachIntentSegment, + GripperCommandMode, + GripperCommandSegment, + JointTrajectorySegment, + TaskMotionPlan, + TaskMotionSegment, + WaitSegment, + make_stable_id, + matrix4, +) +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + MalformedScheduleStreamCommandError, + ScheduleStreamClosedError, + ScheduleStreamCommandSymbols, + ScheduleStreamLimitError, + ScheduleStreamLoweringContext, + ScheduleStreamLoweringLimits, + ScheduleStreamProviderError, + ScheduleStreamTimingError, + UnsupportedScheduleStreamCommandError, +) +from isaac_autodata_interfaces.autonomous.schedulestream.experimental.symbols import ( + load_schedulestream_command_symbols, + native_type_name, +) + +_Selector = Callable[[str, Any | None], Any] +_SymbolProvider = Callable[[str], ScheduleStreamCommandSymbols] +_ResourceCloser = Callable[[Any], None] + + +@dataclass +class _Budget: + limits: ScheduleStreamLoweringLimits + command_nodes: int = 0 + total_samples: int = 0 + source_duration_s: float = 0.0 + has_concurrency: bool = False + + def charge_node(self, path: tuple[int, ...], depth: int) -> None: + if depth > self.limits.max_nesting_depth: + raise ScheduleStreamLimitError(f"command nesting exceeds {self.limits.max_nesting_depth}", path=path) + self.command_nodes += 1 + if self.command_nodes > self.limits.max_command_nodes: + raise ScheduleStreamLimitError(f"command stream exceeds {self.limits.max_command_nodes} nodes", path=path) + + def charge_samples(self, count: int, path: tuple[int, ...]) -> None: + if count < 1: + raise MalformedScheduleStreamCommandError("command contains no samples", path=path) + if count > self.limits.max_samples_per_segment: + raise ScheduleStreamLimitError( + f"segment has {count} samples; limit is {self.limits.max_samples_per_segment}", + path=path, + ) + self.total_samples += count + if self.total_samples > self.limits.max_total_samples: + raise ScheduleStreamLimitError( + f"command stream exceeds {self.limits.max_total_samples} total samples", path=path + ) + + def charge_duration(self, duration_s: float, path: tuple[int, ...]) -> None: + if not math.isfinite(duration_s) or duration_s < 0: + raise ScheduleStreamTimingError("derived duration must be finite and non-negative", path=path) + self.source_duration_s += duration_s + if self.source_duration_s > self.limits.max_duration_s: + raise ScheduleStreamLimitError( + f"source command duration exceeds {self.limits.max_duration_s:g} seconds", path=path + ) + + +@dataclass +class _State: + context: ScheduleStreamLoweringContext + symbols: ScheduleStreamCommandSymbols + budget: _Budget + segments: list[TaskMotionSegment] = field(default_factory=list) + traces: list[DensePlanTrace] = field(default_factory=list) + attachments_by_link: dict[str, str] = field(default_factory=dict) + gripper_by_eef: dict[str, float] = field(default_factory=dict) + + def branch(self) -> _State: + return _State( + context=self.context, + symbols=self.symbols, + budget=self.budget, + segments=self.segments, + traces=self.traces, + attachments_by_link=dict(self.attachments_by_link), + gripper_by_eef=dict(self.gripper_by_eef), + ) + + +@dataclass(frozen=True) +class _Lowered: + terminal_ids: tuple[str, ...] + segment_ids: tuple[str, ...] + duration_s: float + resources: frozenset[str] + + +@dataclass(frozen=True) +class _JointSamples: + joint_names: tuple[str, ...] + positions: tuple[tuple[float, ...], ...] + dt_s: float + held_object: str | None + + +class ScheduleStreamCommandLowerer: + """Select, lazily load, validate, and lower one ScheduleStream command API. + + Selection delegates to :mod:`isaac_autodata_interfaces.motion_planners.curobo.backend_selection` only + when the boundary is first used. Native ScheduleStream modules are loaded one step later and + only for the selected ``custream`` or ``custream2`` application. + """ + + def __init__( + self, + requested_motion_backend: str = "auto", + *, + capabilities: Any | None = None, + selector: _Selector | None = None, + symbols: ScheduleStreamCommandSymbols | None = None, + symbol_provider: _SymbolProvider | None = None, + limits: ScheduleStreamLoweringLimits | None = None, + owned_resource: Any | None = None, + resource_closer: _ResourceCloser | None = None, + ) -> None: + if requested_motion_backend not in ("auto", "curobo_v1", "curobo_v2"): + raise ValueError("requested_motion_backend must be 'auto', 'curobo_v1', or 'curobo_v2'") + if symbols is not None and symbol_provider is not None: + raise ValueError("provide symbols or symbol_provider, not both") + if owned_resource is not None and resource_closer is None: + raise ValueError("owned_resource requires an explicit resource_closer") + self._requested_motion_backend = requested_motion_backend + self._capabilities = capabilities + self._selector = selector or _select_backend + self._symbols = symbols + self._symbol_provider = symbol_provider or load_schedulestream_command_symbols + self._limits = limits or ScheduleStreamLoweringLimits() + self._selection: Any | None = None + self._owned_resource = owned_resource + self._resource_closer = resource_closer + self._closed = False + + @property + def application(self) -> str: + """Selected ScheduleStream application name.""" + + return self._get_application() + + @property + def motion_backend(self) -> str: + """Selected cuRobo backend ID.""" + + selection = self._get_selection() + return _required_selection_text(selection, "motion_backend") + + def dense_trace_from_link_path( + self, + command: Any, + context: ScheduleStreamLoweringContext, + ) -> DensePlanTrace: + """Normalize one native link path into a bounded backend-neutral dense trace.""" + + self._require_open() + symbols = self._get_symbols() + path: tuple[int, ...] = () + if type(command) is not symbols.link_path_type: + raise UnsupportedScheduleStreamCommandError( + f"expected exact {symbols.link_path_type.__name__}, got {native_type_name(command)}", + path=path, + ) + budget = _Budget(self._limits) + budget.charge_node(path, 0) + state = _State( + context=context, + symbols=symbols, + budget=budget, + attachments_by_link=dict(context.initial_attachments_by_link), + ) + return self._extract_dense_trace(command, path, state) + + def lower( + self, + commands: Any, + context: ScheduleStreamLoweringContext, + ) -> TaskMotionPlan: + """Lower a native command stream to a validated :class:`TaskMotionPlan`.""" + + self._require_open() + symbols = self._get_symbols() + state = _State( + context=context, + symbols=symbols, + budget=_Budget(self._limits), + attachments_by_link=dict(context.initial_attachments_by_link), + ) + if isinstance(commands, (list, tuple)): + lowered = self._lower_sequence(commands, (), (), 0, state, in_composite=False) + else: + lowered = self._lower_command(commands, (), (), 0, state, in_composite=False) + if not lowered.segment_ids: + raise MalformedScheduleStreamCommandError("command stream produced no executable segments") + selection = self._get_selection() + application = self._get_application() + metadata = dict(context.metadata) + metadata["schedulestream"] = { + "application": application, + "command_nodes": state.budget.command_nodes, + "dense_trace_count": len(state.traces), + "has_concurrency": state.budget.has_concurrency, + "motion_backend": _required_selection_text(selection, "motion_backend"), + "source_duration_s": state.budget.source_duration_s, + "total_samples": state.budget.total_samples, + } + backend = f"schedulestream_{application}" + backend_version = context.backend_version or _selection_backend_version(selection) + return TaskMotionPlan( + plan_id=make_stable_id( + "plan", + context.request_digest, + context.snapshot_digest, + backend, + context.seed, + ), + request_digest=context.request_digest, + snapshot_digest=context.snapshot_digest, + backend=backend, + backend_version=backend_version, + seed=context.seed, + segments=tuple(state.segments), + goal=context.goal, + metadata=metadata, + ) + + def close(self) -> None: + """Release the explicitly owned native resource exactly once.""" + + if self._closed: + return + self._closed = True + resource = self._owned_resource + closer = self._resource_closer + self._owned_resource = None + self._symbols = None + if resource is not None and closer is not None: + try: + closer(resource) + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamProviderError( + f"failed to close owned ScheduleStream resource ({type(exc).__name__}: {message})" + ) from exc + + def __enter__(self) -> ScheduleStreamCommandLowerer: + self._require_open() + return self + + def __exit__(self, exc_type: Any, exc: Any, traceback: Any) -> None: + self.close() + + def _get_selection(self) -> Any: + self._require_open() + if self._selection is None: + self._selection = self._selector(self._requested_motion_backend, self._capabilities) + _required_selection_text(self._selection, "motion_backend") + _required_selection_text(self._selection, "schedulestream_application") + return self._selection + + def _get_application(self) -> str: + application = _required_selection_text(self._get_selection(), "schedulestream_application") + if application not in ("custream", "custream2"): + raise ScheduleStreamProviderError( + f"compatibility selector returned unsupported application {application!r}" + ) + return application + + def _get_symbols(self) -> ScheduleStreamCommandSymbols: + self._require_open() + application = self._get_application() + if self._symbols is None: + self._symbols = self._symbol_provider(application) + if not isinstance(self._symbols, ScheduleStreamCommandSymbols): + raise ScheduleStreamProviderError("symbol provider did not return ScheduleStreamCommandSymbols") + if self._symbols.application != application: + raise ScheduleStreamProviderError( + f"symbol provider returned {self._symbols.application!r} for selected {application!r}" + ) + return self._symbols + + def _require_open(self) -> None: + if self._closed: + raise ScheduleStreamClosedError("ScheduleStream boundary is closed") + + def _lower_sequence( + self, + commands: Sequence[Any], + dependencies: tuple[str, ...], + path: tuple[int, ...], + depth: int, + state: _State, + *, + in_composite: bool, + ) -> _Lowered: + if isinstance(commands, (str, bytes)): + raise MalformedScheduleStreamCommandError("commands must be a sequence of command objects", path=path) + if not commands: + raise MalformedScheduleStreamCommandError("sequential command container must not be empty", path=path) + if len(commands) > state.budget.limits.max_command_nodes: + raise ScheduleStreamLimitError("sequential command container is too large", path=path) + current_dependencies = dependencies + segment_ids: list[str] = [] + resources: set[str] = set() + duration_s = 0.0 + index = 0 + while index < len(commands): + command = commands[index] + if type(command) is state.symbols.configuration_type: + run: list[tuple[Any, tuple[int, ...]]] = [] + while index < len(commands) and type(commands[index]) is state.symbols.configuration_type: + run.append((commands[index], path + (index,))) + index += 1 + lowered = self._lower_configurations( + run, + current_dependencies, + depth, + state, + in_composite=in_composite, + ) + else: + lowered = self._lower_command( + command, + current_dependencies, + path + (index,), + depth, + state, + in_composite=in_composite, + ) + index += 1 + current_dependencies = lowered.terminal_ids + segment_ids.extend(lowered.segment_ids) + resources.update(lowered.resources) + duration_s += lowered.duration_s + return _Lowered(current_dependencies, tuple(segment_ids), duration_s, frozenset(resources)) + + def _lower_command( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + depth: int, + state: _State, + *, + in_composite: bool, + ) -> _Lowered: + state.budget.charge_node(path, depth) + command_type = type(command) + symbols = state.symbols + if command_type is symbols.commands_type: + children = _native_children(command, path) + return self._lower_sequence( + children, + dependencies, + path, + depth + 1, + state, + in_composite=in_composite, + ) + if command_type is symbols.composite_type: + return self._lower_composite(command, dependencies, path, depth + 1, state) + if command_type is symbols.configuration_type: + return self._lower_configurations( + [(command, path)], dependencies, depth, state, in_composite=in_composite, charged=True + ) + if command_type is symbols.link_path_type: + return self._lower_link_path(command, dependencies, path, state) + if command_type is symbols.trajectory_type: + return self._lower_trajectory(command, dependencies, path, state) + if command_type is symbols.open_type: + return self._lower_gripper(command, dependencies, path, state, GripperCommandMode.OPEN) + if command_type is symbols.close_type: + return self._lower_gripper(command, dependencies, path, state, GripperCommandMode.CLOSE) + if command_type is symbols.attach_type: + return self._lower_attach(command, dependencies, path, state, in_composite=in_composite) + if command_type is symbols.detach_type: + return self._lower_detach(command, dependencies, path, state, in_composite=in_composite) + supported = sorted({ + symbols.commands_type.__name__, + symbols.composite_type.__name__, + symbols.configuration_type.__name__, + symbols.trajectory_type.__name__, + symbols.link_path_type.__name__, + symbols.open_type.__name__, + symbols.close_type.__name__, + symbols.attach_type.__name__, + symbols.detach_type.__name__, + }) + raise UnsupportedScheduleStreamCommandError( + f"unsupported exact native type {native_type_name(command)}; supported: {supported}", path=path + ) + + def _lower_configurations( + self, + commands: list[tuple[Any, tuple[int, ...]]], + dependencies: tuple[str, ...], + depth: int, + state: _State, + *, + in_composite: bool, + charged: bool = False, + ) -> _Lowered: + del in_composite + samples: list[tuple[_JointSamples, tuple[int, ...]]] = [] + for index, (command, command_path) in enumerate(commands): + if not charged or index > 0: + state.budget.charge_node(command_path, depth) + sample = self._extract_joint_samples(command, command_path, state, configuration=True) + if len(sample.positions) != 1: + raise MalformedScheduleStreamCommandError( + "Configuration must contain exactly one joint sample", path=command_path + ) + samples.append((sample, command_path)) + + current_dependencies = dependencies + segment_ids: list[str] = [] + resources: set[str] = set() + total_duration = 0.0 + index = 0 + while index < len(samples): + first = samples[index][0] + end = index + 1 + while end < len(samples): + candidate = samples[end][0] + if ( + candidate.joint_names != first.joint_names + or not _same_dt(candidate.dt_s, first.dt_s) + or candidate.held_object != first.held_object + ): + break + end += 1 + group = samples[index:end] + positions = tuple(item.positions[0] for item, _ in group) + group_path = group[0][1] + state.budget.charge_samples(len(group), group_path) + duration = len(group) * first.dt_s + state.budget.charge_duration(duration, group_path) + if len(group) >= 2 and all(position == positions[0] for position in positions[1:]): + segment_id = make_stable_id( + "wait", state.context.request_digest, group_path, len(group), first.joint_names, positions[0] + ) + segment: TaskMotionSegment = WaitSegment( + segment_id=segment_id, + depends_on=current_dependencies, + duration_s=duration, + steps=len(group), + metadata={ + "held_object": first.held_object, + "joint_names": list(first.joint_names), + "joint_position": list(positions[0]), + "source_command": "Configuration", + "source_path": list(group_path), + "step_dt_s": first.dt_s, + }, + ) + else: + segment_id = make_stable_id( + "joint", state.context.request_digest, group_path, first.joint_names, positions + ) + segment = JointTrajectorySegment( + segment_id=segment_id, + depends_on=current_dependencies, + duration_s=duration, + joint_names=first.joint_names, + positions=positions, + timestamps_s=tuple(sample_index * first.dt_s for sample_index in range(len(group))), + attached_object=first.held_object, + metadata={ + "source_command": "Configuration", + "source_path": list(group_path), + "step_dt_s": first.dt_s, + }, + ) + state.segments.append(segment) + current_dependencies = (segment_id,) + segment_ids.append(segment_id) + resources.update(f"joint:{name}" for name in first.joint_names) + total_duration += duration + index = end + return _Lowered(current_dependencies, tuple(segment_ids), total_duration, frozenset(resources)) + + def _lower_trajectory( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + state: _State, + ) -> _Lowered: + samples = self._extract_joint_samples(command, path, state, configuration=False) + count = len(samples.positions) + state.budget.charge_samples(count, path) + duration = max(0, count - 1) * samples.dt_s + state.budget.charge_duration(duration, path) + segment_id = make_stable_id("joint", state.context.request_digest, path, samples.joint_names, samples.positions) + segment = JointTrajectorySegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=duration, + joint_names=samples.joint_names, + positions=samples.positions, + timestamps_s=tuple(index * samples.dt_s for index in range(count)), + attached_object=samples.held_object, + metadata={ + "source_command": "Trajectory", + "source_path": list(path), + "step_dt_s": samples.dt_s, + }, + ) + state.segments.append(segment) + resources = {f"joint:{name}" for name in samples.joint_names} + arm = _optional_command_arm(command, None, path) + if arm is not None: + resources.add(f"arm:{arm}") + return _Lowered((segment_id,), (segment_id,), duration, frozenset(resources)) + + def _lower_link_path( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + state: _State, + ) -> _Lowered: + trace = self._extract_dense_trace(command, path, state) + state.traces.append(trace) + duration = len(trace.poses) * trace.step_dt_s + state.budget.charge_duration(duration, path) + link = _optional_text_attribute(command, "link", path) + eef_name, arm = self._resolve_eef(command, link, path, state.context) + held_objects = self._held_objects(command, path, state.context) + segment_id = make_stable_id("cartesian", state.context.request_digest, path, eef_name, trace.poses) + segment = CartesianTrajectorySegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=duration, + eef_name=eef_name, + frame=trace.frame, + poses=trace.poses, + metadata={ + "held_objects": list(held_objects), + "source_command": state.symbols.link_path_type.__name__, + "source_link": link, + "source_path": list(path), + "step_dt_s": trace.step_dt_s, + }, + ) + state.segments.append(segment) + resources = {f"eef:{eef_name}", f"arm:{arm}"} + return _Lowered((segment_id,), (segment_id,), duration, frozenset(resources)) + + def _extract_dense_trace( + self, + command: Any, + path: tuple[int, ...], + state: _State, + ) -> DensePlanTrace: + count = _path_length(command, state.symbols.application, path) + state.budget.charge_samples(count, path) + dt_s = _command_dt(command, path, state.context.step_dt_s) + link = _optional_text_attribute(command, "link", path) + eef_name, _ = self._resolve_eef(command, link, path, state.context) + poses = [] + for index in range(count): + try: + native_pose = command[index] if state.symbols.application == "custream" else command.pose(index) + native_matrix = state.symbols.pose_to_matrix(native_pose) + except Exception as exc: + raise _malformed_from_exception("failed to read link-path pose", path + (index,), exc) from exc + matrix_rows = _numeric_matrix(native_matrix, 4, 4, path + (index,)) + try: + poses.append(matrix4(matrix_rows, f"commands{path}.poses[{index}]")) + except ValueError as exc: + raise MalformedScheduleStreamCommandError(str(exc), path=path + (index,)) from exc + gripper = state.gripper_by_eef.get(eef_name, state.context.initial_gripper_value) + return DensePlanTrace( + eef_name=eef_name, + frame=state.context.frame, + poses=tuple(poses), + gripper_values=(gripper,) * count, + step_dt_s=dt_s, + ) + + def _lower_gripper( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + state: _State, + mode: GripperCommandMode, + ) -> _Lowered: + dt_s = _command_dt(command, path, state.context.step_dt_s) + steps = _positive_step_count(command, path, default=1) + state.budget.charge_samples(steps, path) + duration = steps * dt_s + state.budget.charge_duration(duration, path) + eef_name, arm = self._resolve_eef(command, None, path, state.context) + segment_id = make_stable_id("gripper", state.context.request_digest, path, eef_name, mode.value, steps) + segment = GripperCommandSegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=duration, + eef_name=eef_name, + command=mode, + settle_steps=steps, + metadata={ + "source_arm": arm, + "source_command": type(command).__name__, + "source_path": list(path), + "step_dt_s": dt_s, + }, + ) + state.segments.append(segment) + state.gripper_by_eef[eef_name] = 1.0 if mode is GripperCommandMode.OPEN else -1.0 + return _Lowered( + (segment_id,), + (segment_id,), + duration, + frozenset({f"eef:{eef_name}", f"arm:{arm}"}), + ) + + def _lower_attach( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + state: _State, + *, + in_composite: bool, + ) -> _Lowered: + if in_composite: + raise UnsupportedScheduleStreamCommandError( + "attachment transitions inside Composite are ambiguous and are rejected", path=path + ) + source_object = _required_text_attribute(command, "obj", path) + link_field = "parent" if state.symbols.application == "custream" else "link" + link = _required_text_attribute(command, link_field, path) + eef_name, arm = self._resolve_eef(command, link, path, state.context) + object_name = _resolve_object_name(source_object, path, state.context) + if link in state.attachments_by_link: + raise MalformedScheduleStreamCommandError( + f"link {link!r} is already tracked as holding {state.attachments_by_link[link]!r}", path=path + ) + if object_name in state.attachments_by_link.values(): + raise MalformedScheduleStreamCommandError( + f"object {object_name!r} is already attached to another link", path=path + ) + dt_s = _command_dt(command, path, state.context.step_dt_s) + steps = _positive_step_count(command, path, default=1) + state.budget.charge_samples(steps, path) + duration = steps * dt_s + state.budget.charge_duration(duration, path) + segment_id = make_stable_id("attach", state.context.request_digest, path, eef_name, object_name) + segment = AttachIntentSegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=duration, + eef_name=eef_name, + object_name=object_name, + verifier=state.context.attachment_verifier, + metadata={ + "source_arm": arm, + "source_link": link, + "source_object": source_object, + "source_path": list(path), + }, + ) + state.segments.append(segment) + state.attachments_by_link[link] = object_name + return _Lowered( + (segment_id,), + (segment_id,), + duration, + frozenset({f"eef:{eef_name}", f"arm:{arm}"}), + ) + + def _lower_detach( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + state: _State, + *, + in_composite: bool, + ) -> _Lowered: + if in_composite: + raise UnsupportedScheduleStreamCommandError( + "attachment transitions inside Composite are ambiguous and are rejected", path=path + ) + link_field = "parent" if state.symbols.application == "custream" else "link" + link = _required_text_attribute(command, link_field, path) + eef_name, arm = self._resolve_eef(command, link, path, state.context) + object_name = state.attachments_by_link.get(link) + if object_name is None: + raise UnsupportedScheduleStreamCommandError( + f"Detach({link!r}) does not name its object; bind initial_attachments_by_link or precede it " + "with a tracked Attach", + path=path, + ) + dt_s = _command_dt(command, path, state.context.step_dt_s) + steps = _positive_step_count(command, path, default=1) + state.budget.charge_samples(steps, path) + duration = steps * dt_s + state.budget.charge_duration(duration, path) + segment_id = make_stable_id("detach", state.context.request_digest, path, eef_name, object_name) + segment = DetachIntentSegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=duration, + eef_name=eef_name, + object_name=object_name, + verifier=state.context.attachment_verifier, + metadata={"source_arm": arm, "source_link": link, "source_path": list(path)}, + ) + state.segments.append(segment) + del state.attachments_by_link[link] + return _Lowered( + (segment_id,), + (segment_id,), + duration, + frozenset({f"eef:{eef_name}", f"arm:{arm}"}), + ) + + def _lower_composite( + self, + command: Any, + dependencies: tuple[str, ...], + path: tuple[int, ...], + depth: int, + state: _State, + ) -> _Lowered: + children = _native_children(command, path) + if len(children) > state.budget.limits.max_composite_width: + raise ScheduleStreamLimitError( + f"Composite width exceeds {state.budget.limits.max_composite_width}", path=path + ) + if not children: + dt_s = _command_dt(command, path, state.context.step_dt_s) + state.budget.charge_samples(1, path) + state.budget.charge_duration(dt_s, path) + segment_id = make_stable_id("wait", state.context.request_digest, path, "empty_composite") + segment = WaitSegment( + segment_id=segment_id, + depends_on=dependencies, + duration_s=dt_s, + steps=1, + metadata={ + "source_command": "Composite", + "source_path": list(path), + "step_dt_s": dt_s, + }, + ) + state.segments.append(segment) + return _Lowered((segment_id,), (segment_id,), dt_s, frozenset()) + if len(children) == 1: + return self._lower_command(children[0], dependencies, path + (0,), depth, state, in_composite=True) + + state.budget.has_concurrency = True + base_attachments = dict(state.attachments_by_link) + base_grippers = dict(state.gripper_by_eef) + changed_grippers: set[str] = set() + occupied_resources: set[str] = set() + terminal_ids: list[str] = [] + member_ids: list[str] = [] + duration = 0.0 + branch_grippers: dict[str, float] = {} + for index, child in enumerate(children): + branch = state.branch() + branch.attachments_by_link = dict(base_attachments) + branch.gripper_by_eef = dict(base_grippers) + lowered = self._lower_command( + child, + dependencies, + path + (index,), + depth, + branch, + in_composite=True, + ) + conflict = occupied_resources & set(lowered.resources) + if conflict: + raise UnsupportedScheduleStreamCommandError( + f"Composite branches command overlapping resources {sorted(conflict)}", path=path + ) + occupied_resources.update(lowered.resources) + terminal_ids.extend(lowered.terminal_ids) + member_ids.extend(lowered.segment_ids) + duration = max(duration, lowered.duration_s) + if branch.attachments_by_link != base_attachments: + raise UnsupportedScheduleStreamCommandError( + "Composite attachment state cannot be merged deterministically", path=path + ) + for eef_name, value in branch.gripper_by_eef.items(): + if base_grippers.get(eef_name, state.context.initial_gripper_value) == value: + continue + if eef_name in changed_grippers: + raise UnsupportedScheduleStreamCommandError( + f"Composite changes gripper {eef_name!r} in more than one branch", path=path + ) + changed_grippers.add(eef_name) + branch_grippers[eef_name] = value + state.gripper_by_eef.update(branch_grippers) + group_id = make_stable_id("concurrent", state.context.request_digest, path, member_ids) + group = ConcurrentGroupSegment( + segment_id=group_id, + depends_on=tuple(dict.fromkeys(terminal_ids)), + duration_s=duration, + member_segment_ids=tuple(member_ids), + metadata={ + "source_command": "Composite", + "source_path": list(path), + "width": len(children), + }, + ) + state.segments.append(group) + return _Lowered( + (group_id,), + tuple(member_ids) + (group_id,), + duration, + frozenset(occupied_resources), + ) + + def _extract_joint_samples( + self, + command: Any, + path: tuple[int, ...], + state: _State, + *, + configuration: bool, + ) -> _JointSamples: + try: + joint_state = command.joint_state + raw_names = command.joints if hasattr(command, "joints") else joint_state.joint_names + raw_positions = joint_state.position + except Exception as exc: + raise _malformed_from_exception("failed to read joint state", path, exc) from exc + names = _joint_names(raw_names, path, state.budget.limits.max_joints) + positions = _numeric_rows( + raw_positions, + path, + max_rows=(1 if configuration else state.budget.limits.max_samples_per_segment), + expected_columns=len(names), + ) + dt_s = _command_dt(command, path, state.context.step_dt_s) + held_objects = self._held_objects(command, path, state.context) + if len(held_objects) > 1: + raise UnsupportedScheduleStreamCommandError( + "TaskMotionPlan v1 can associate at most one attached object with a joint trajectory", + path=path, + ) + return _JointSamples(names, positions, dt_s, held_objects[0] if held_objects else None) + + def _held_objects( + self, + command: Any, + path: tuple[int, ...], + context: ScheduleStreamLoweringContext, + ) -> tuple[str, ...]: + raw: Any = None + for attribute in ("holding", "grasped", "grasps"): + try: + raw = getattr(command, attribute) + except AttributeError: + continue + except Exception as exc: + raise _malformed_from_exception(f"failed to read {attribute}", path, exc) from exc + else: + break + if raw is None: + return () + if not isinstance(raw, (list, tuple)): + raise MalformedScheduleStreamCommandError("held-object collection must be a list or tuple", path=path) + if len(raw) > 64: + raise ScheduleStreamLimitError("held-object collection exceeds 64 entries", path=path) + names = [] + for item in raw: + source_name = item if isinstance(item, str) else _required_text_attribute(item, "obj", path) + names.append(_resolve_object_name(source_name, path, context)) + if len(names) != len(set(names)): + raise MalformedScheduleStreamCommandError("held-object collection contains duplicates", path=path) + return tuple(names) + + def _resolve_eef( + self, + command: Any, + link: str | None, + path: tuple[int, ...], + context: ScheduleStreamLoweringContext, + ) -> tuple[str, str]: + if link is not None and link in context.eef_by_link: + arm = _optional_command_arm(command, link, path) or link + return context.eef_by_link[link], arm + arm = _optional_command_arm(command, link, path) + if arm is None: + raise UnsupportedScheduleStreamCommandError( + "command is not associated with a robot arm/end effector", path=path + ) + if context.eef_by_arm: + if arm not in context.eef_by_arm: + raise UnsupportedScheduleStreamCommandError(f"native arm {arm!r} has no eef_by_arm binding", path=path) + return context.eef_by_arm[arm], arm + return context.eef_name, arm + + +def _select_backend(requested_motion_backend: str, capabilities: Any | None) -> Any: + from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import select_schedulestream_backend + + return select_schedulestream_backend(requested_motion_backend, capabilities) + + +def _required_selection_text(selection: Any, attribute: str) -> str: + value = getattr(selection, attribute, None) + if not isinstance(value, str) or not value: + raise ScheduleStreamProviderError(f"compatibility selector returned no valid {attribute}") + return value + + +def _selection_backend_version(selection: Any) -> str: + capabilities = getattr(selection, "capabilities", None) + identity = getattr(capabilities, "schedulestream", None) + version = getattr(identity, "version", None) + commit = getattr(identity, "source_commit", None) + if isinstance(version, str) and version: + if isinstance(commit, str) and commit: + return f"{version}+{commit[:12]}" + return version + return "unknown" + + +def _native_children(command: Any, path: tuple[int, ...]) -> tuple[Any, ...]: + try: + children = command.commands + except Exception as exc: + raise _malformed_from_exception("command container has no readable commands", path, exc) from exc + if not isinstance(children, (list, tuple)): + raise MalformedScheduleStreamCommandError("native commands must be stored as a list or tuple", path=path) + return tuple(children) + + +def _required_text_attribute(value: Any, attribute: str, path: tuple[int, ...]) -> str: + try: + result = getattr(value, attribute) + except Exception as exc: + raise _malformed_from_exception(f"missing or unreadable {attribute}", path, exc) from exc + if not isinstance(result, str) or not result.strip() or len(result) > 512 or "\x00" in result: + raise MalformedScheduleStreamCommandError(f"{attribute} must be a non-empty bounded string", path=path) + return result + + +def _optional_text_attribute(value: Any, attribute: str, path: tuple[int, ...]) -> str | None: + try: + result = getattr(value, attribute) + except AttributeError: + return None + except Exception as exc: + raise _malformed_from_exception(f"unreadable {attribute}", path, exc) from exc + if result is None: + return None + if not isinstance(result, str) or not result.strip() or len(result) > 512 or "\x00" in result: + raise MalformedScheduleStreamCommandError(f"{attribute} must be null or a non-empty bounded string", path=path) + return result + + +def _optional_command_arm(command: Any, link: str | None, path: tuple[int, ...]) -> str | None: + try: + arm = getattr(command, "arm") + except AttributeError: + arm = None + except Exception as exc: + raise _malformed_from_exception("failed to resolve native arm", path, exc) from exc + if arm is None and link is not None: + try: + resolver = command.world.get_link_arm + arm = resolver(link) + except Exception as exc: + raise _malformed_from_exception("failed to map native link to arm", path, exc) from exc + if arm is None: + return None + if not isinstance(arm, str) or not arm.strip() or len(arm) > 512 or "\x00" in arm: + raise MalformedScheduleStreamCommandError("native arm must be a non-empty bounded string", path=path) + return arm + + +def _resolve_object_name( + source_name: str, + path: tuple[int, ...], + context: ScheduleStreamLoweringContext, +) -> str: + if context.object_name_map: + if source_name not in context.object_name_map: + raise UnsupportedScheduleStreamCommandError( + f"native object {source_name!r} has no object_name_map binding", path=path + ) + return context.object_name_map[source_name] + return source_name + + +def _command_dt(command: Any, path: tuple[int, ...], expected_dt_s: float) -> float: + try: + raw_dt = getattr(command, "time_step") + except AttributeError: + try: + raw_dt = command.world.time_step + except Exception as exc: + raise _malformed_from_exception("command has no readable time step", path, exc) from exc + except Exception as exc: + raise _malformed_from_exception("failed to read command time step", path, exc) from exc + if isinstance(raw_dt, bool): + raise ScheduleStreamTimingError("command time step must be numeric", path=path) + try: + dt_s = float(raw_dt) + except (TypeError, ValueError, OverflowError) as exc: + raise ScheduleStreamTimingError("command time step must be numeric", path=path) from exc + if not math.isfinite(dt_s) or dt_s <= 0: + raise ScheduleStreamTimingError("command time step must be positive and finite", path=path) + tolerance = max(1e-9, abs(expected_dt_s) * 1e-6) + if abs(dt_s - expected_dt_s) > tolerance: + raise ScheduleStreamTimingError( + f"native time step {dt_s:g}s does not match executor time step {expected_dt_s:g}s; " + "resample before lowering", + path=path, + ) + return dt_s + + +def _positive_step_count(command: Any, path: tuple[int, ...], *, default: int) -> int: + try: + value = getattr(command, "num_steps") + except AttributeError: + value = default + except Exception as exc: + raise _malformed_from_exception("failed to read num_steps", path, exc) from exc + if isinstance(value, bool) or not isinstance(value, int) or value < 1: + raise MalformedScheduleStreamCommandError("num_steps must be a positive integer", path=path) + return value + + +def _path_length(command: Any, application: str, path: tuple[int, ...]) -> int: + try: + value = len(command) if application == "custream" else command.length + except Exception as exc: + raise _malformed_from_exception("failed to read link-path length", path, exc) from exc + if isinstance(value, bool) or not isinstance(value, int) or value < 1: + raise MalformedScheduleStreamCommandError("link-path length must be a positive integer", path=path) + return value + + +def _joint_names(raw_names: Any, path: tuple[int, ...], max_joints: int) -> tuple[str, ...]: + if not isinstance(raw_names, (list, tuple)): + raise MalformedScheduleStreamCommandError("joint names must be a list or tuple", path=path) + if not raw_names: + raise MalformedScheduleStreamCommandError("joint names must not be empty", path=path) + if len(raw_names) > max_joints: + raise ScheduleStreamLimitError(f"joint count exceeds {max_joints}", path=path) + names = [] + for name in raw_names: + if not isinstance(name, str) or not name.strip() or len(name) > 512 or "\x00" in name: + raise MalformedScheduleStreamCommandError("joint names must be bounded non-empty strings", path=path) + names.append(name) + if len(names) != len(set(names)): + raise MalformedScheduleStreamCommandError("joint names must be unique", path=path) + return tuple(names) + + +def _numeric_rows( + value: Any, + path: tuple[int, ...], + *, + max_rows: int, + expected_columns: int, +) -> tuple[tuple[float, ...], ...]: + shape = _native_shape(value, path) + if shape is not None: + if len(shape) != 2: + raise MalformedScheduleStreamCommandError(f"joint positions must have rank 2, got shape {shape}", path=path) + if shape[0] < 1 or shape[0] > max_rows or shape[1] != expected_columns: + raise MalformedScheduleStreamCommandError( + f"joint positions shape {shape} violates [1..{max_rows}, {expected_columns}]", path=path + ) + materialized = _to_builtin(value, path) + if not isinstance(materialized, (list, tuple)): + raise MalformedScheduleStreamCommandError("joint positions must be a two-dimensional array", path=path) + if not materialized or len(materialized) > max_rows: + raise ScheduleStreamLimitError(f"joint sample count must be in [1, {max_rows}]", path=path) + rows = [] + for row in materialized: + if not isinstance(row, (list, tuple)) or len(row) != expected_columns: + raise MalformedScheduleStreamCommandError( + f"every joint row must contain {expected_columns} values", path=path + ) + rows.append(tuple(_finite_number(item, path) for item in row)) + return tuple(rows) + + +def _numeric_matrix( + value: Any, + rows: int, + columns: int, + path: tuple[int, ...], +) -> tuple[tuple[float, ...], ...]: + shape = _native_shape(value, path) + if shape is not None and shape != (rows, columns): + raise MalformedScheduleStreamCommandError( + f"pose matrix must have shape [{rows}, {columns}], got {shape}", path=path + ) + materialized = _to_builtin(value, path) + if not isinstance(materialized, (list, tuple)) or len(materialized) != rows: + raise MalformedScheduleStreamCommandError(f"pose matrix must have shape [{rows}, {columns}]", path=path) + result = [] + for row in materialized: + if not isinstance(row, (list, tuple)) or len(row) != columns: + raise MalformedScheduleStreamCommandError(f"pose matrix must have shape [{rows}, {columns}]", path=path) + result.append(tuple(_finite_number(item, path) for item in row)) + return tuple(result) + + +def _native_shape(value: Any, path: tuple[int, ...]) -> tuple[int, ...] | None: + try: + raw_shape = getattr(value, "shape") + except AttributeError: + return None + except Exception as exc: + raise _malformed_from_exception("failed to inspect native array shape", path, exc) from exc + try: + shape = tuple(int(item) for item in raw_shape) + except (TypeError, ValueError, OverflowError) as exc: + raise MalformedScheduleStreamCommandError("native array shape is invalid", path=path) from exc + if any(item < 0 for item in shape): + raise MalformedScheduleStreamCommandError("native array shape cannot be negative", path=path) + return shape + + +def _to_builtin(value: Any, path: tuple[int, ...]) -> Any: + result = value + for method_name in ("detach", "cpu", "tolist"): + method = getattr(result, method_name, None) + if callable(method): + try: + result = method() + except Exception as exc: + raise _malformed_from_exception( + f"failed to materialize native array via {method_name}", path, exc + ) from exc + return result + + +def _finite_number(value: Any, path: tuple[int, ...]) -> float: + if isinstance(value, bool): + raise MalformedScheduleStreamCommandError("numeric arrays must not contain booleans", path=path) + try: + result = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise MalformedScheduleStreamCommandError("numeric arrays must contain numbers", path=path) from exc + if not math.isfinite(result): + raise MalformedScheduleStreamCommandError("numeric arrays must contain only finite values", path=path) + return result + + +def _same_dt(first: float, second: float) -> bool: + return abs(first - second) <= max(1e-9, abs(first) * 1e-6) + + +def _malformed_from_exception( + message: str, + path: tuple[int, ...], + exc: Exception, +) -> MalformedScheduleStreamCommandError: + detail = str(exc).replace("\n", " ")[:300] + return MalformedScheduleStreamCommandError(f"{message} ({type(exc).__name__}: {detail})", path=path) diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/experimental/symbols.py b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/symbols.py new file mode 100644 index 0000000..fa99af7 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/experimental/symbols.py @@ -0,0 +1,62 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Lazy native symbols for experimental ScheduleStream v1/v2 command lowering.""" + +from __future__ import annotations + +import importlib +from typing import Any + +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + ScheduleStreamCommandSymbols, + ScheduleStreamImportError, +) + + +def load_schedulestream_command_symbols(application: str) -> ScheduleStreamCommandSymbols: + """Import only the selected ScheduleStream application and return its command surface. + + Args: + application: ``custream`` for cuRobo v1 or ``custream2`` for cuRobo v2. + """ + + if application not in ("custream", "custream2"): + raise ScheduleStreamImportError( + f"unknown ScheduleStream application {application!r}; expected 'custream' or 'custream2'" + ) + command_module_name = f"schedulestream.applications.{application}.command" + state_module_name = f"schedulestream.applications.{application}.state" + utils_module_name = f"schedulestream.applications.{application}.utils" + try: + command = importlib.import_module(command_module_name) + state = importlib.import_module(state_module_name) + utils = importlib.import_module(utils_module_name) + link_path_type = command.ArmPath if application == "custream" else command.LinkPath + return ScheduleStreamCommandSymbols( + application=application, + commands_type=command.Commands, + composite_type=command.Composite, + configuration_type=state.Configuration, + trajectory_type=command.Trajectory, + link_path_type=link_path_type, + open_type=command.Open, + close_type=command.Close, + attach_type=command.Attach, + detach_type=command.Detach, + pose_to_matrix=utils.matrix_from_pose, + ) + except Exception as exc: + message = str(exc).replace("\n", " ")[:500] + raise ScheduleStreamImportError( + f"failed to load reviewed {application} command API ({type(exc).__name__}: {message})" + ) from exc + + +def native_type_name(value: Any) -> str: + """Return a bounded native type name without invoking an object's representation.""" + + value_type = type(value) + return f"{value_type.__module__}.{value_type.__qualname__}"[:512] diff --git a/isaac_autodata_interfaces/autonomous/schedulestream/goal_lowering.py b/isaac_autodata_interfaces/autonomous/schedulestream/goal_lowering.py new file mode 100644 index 0000000..cf42069 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/schedulestream/goal_lowering.py @@ -0,0 +1,104 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Translate planner-neutral goals into ScheduleStream formulas.""" + +from __future__ import annotations + +from collections.abc import Callable, Iterable +from dataclasses import dataclass +from typing import Any + +from isaac_autodata_core.autonomous.task_motion import GoalPredicate + + +class GoalCompilationError(ValueError): + """Raised when a planner-neutral goal cannot be represented by the selected domain.""" + + +@dataclass(frozen=True) +class ScheduleStreamGoalSymbols: + """Minimal injected ScheduleStream language surface used by the pure goal compiler.""" + + attached_equals: Callable[[str, str], Any] + holding_equals: Callable[[str, str], Any] + + +def compile_schedulestream_goal( + predicates: Iterable[GoalPredicate], + symbols: ScheduleStreamGoalSymbols, + *, + arm: str, + supported_relations: frozenset[str] = frozenset({"on", "in", "at", "holding"}), +) -> Any: + """Compile planner-neutral predicates into one ScheduleStream conjunction. + + Args: + predicates: Goal clauses resolved to live scene IDs. + symbols: Injected ScheduleStream language constructors. + arm: ScheduleStream arm identifier used by ``holding``. + supported_relations: Relations admitted by the selected domain and runtime profile. + + Returns: + A ScheduleStream formula object. Its concrete type remains behind the adapter boundary. + """ + + clauses: list[Any] = [] + for index, predicate in enumerate(predicates): + relation = predicate.relation.lower() + if relation not in supported_relations: + raise GoalCompilationError( + f"goal[{index}] relation {predicate.relation!r} is not supported by this " + f"ScheduleStream capability; supported: {sorted(supported_relations)}" + ) + if relation == "holding": + if predicate.target is not None: + raise GoalCompilationError(f"goal[{index}] holding is unary and must not define target") + clauses.append(symbols.holding_equals(arm, predicate.subject)) + continue + if predicate.target is None: + raise GoalCompilationError(f"goal[{index}] relation {relation!r} requires target") + # ScheduleStream represents final support/containment/at-placement facts through + # Attached(object) == destination. Placement streams test geometric feasibility. + clauses.append(symbols.attached_equals(predicate.subject, predicate.target)) + + if not clauses: + raise GoalCompilationError("ScheduleStream goal must contain at least one predicate") + goal = clauses[0] + for clause in clauses[1:]: + goal = goal & clause + return goal + + +def load_schedulestream_goal_symbols(application: str) -> ScheduleStreamGoalSymbols: + """Lazily load goal symbols for ``custream`` (v1) or ``custream2`` (v2). + + Args: + application: ScheduleStream manipulation application selected by the runtime check. + + Returns: + The native formula constructors wrapped behind an import-safe interface. + """ + + if application == "custream": + from schedulestream.applications.custream.example import Attached, Holding + elif application == "custream2": + from schedulestream.applications.custream2.tamp import Attached, Holding + else: + raise GoalCompilationError( + f"Unknown ScheduleStream application {application!r}; expected 'custream' or 'custream2'" + ) + return ScheduleStreamGoalSymbols( + attached_equals=lambda subject, target: Attached(subject) == target, + holding_equals=lambda arm, subject: Holding(arm) == subject, + ) + + +__all__ = [ + "GoalCompilationError", + "ScheduleStreamGoalSymbols", + "compile_schedulestream_goal", + "load_schedulestream_goal_symbols", +] diff --git a/isaac_autodata_interfaces/autonomous/task_compiler.py b/isaac_autodata_interfaces/autonomous/task_compiler.py new file mode 100644 index 0000000..1f53852 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/task_compiler.py @@ -0,0 +1,78 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Compile a validated task request through an injectable Arena intent bridge.""" + +from __future__ import annotations + +from pathlib import Path + +from isaac_autodata_interfaces.autonomous.arena_bridge import ArenaIntentBridge, LazyArenaIntentBridge +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.task_request import TASK_REQUEST_SCHEMA_VERSION, load_task_request +from isaac_autodata_interfaces.autonomous.task_request_types import ( + CompiledTaskRequest, + ResolvedOutputConfig, + TaskRequest, +) + +TASK_COMPILER_VERSION = "1.0.0" + + +def compile_loaded_task_request( + request: TaskRequest, + *, + bridge: ArenaIntentBridge | None = None, +) -> CompiledTaskRequest: + """Compile and link a task request's opaque Arena intent into a pure runtime input.""" + + selected_bridge = bridge if bridge is not None else LazyArenaIntentBridge() + try: + arena_result = selected_bridge.compile_and_link( + request.environment_intent, + seed=request.generation.seed, + ) + except AutonomousValidationError: + raise + except Exception as exc: + safe_message = str(exc).replace("\n", " ")[:2048] + raise AutonomousValidationError([ + ValidationIssue( + ("environment", "intent"), + "arena_bridge_failed", + f"Arena intent bridge failed with {type(exc).__name__}: {safe_message}", + ) + ]) from None + + request_dir = request.source_path.parent.resolve(strict=False) + dataset = (request_dir / request.output.dataset).resolve(strict=False) + run_log = None if request.output.run_log is None else (request_dir / request.output.run_log).resolve(strict=False) + output = ResolvedOutputConfig( + dataset=dataset, + keep_failed=request.output.keep_failed, + run_log=run_log, + ) + return CompiledTaskRequest( + schema_version=TASK_REQUEST_SCHEMA_VERSION, + compiler_version=TASK_COMPILER_VERSION, + name=request.name, + canonical_request_json=request.canonical_json(), + request_digest=request.digest, + planner=request.planner, + generation=request.generation, + output=output, + arena=arena_result, + source_dataset_path=None, + ) + + +def compile_task_request( + path: str | Path, + *, + bridge: ArenaIntentBridge | None = None, +) -> CompiledTaskRequest: + """Load, validate, compile, and link one AutoData v1 task request file.""" + + return compile_loaded_task_request(load_task_request(path), bridge=bridge) diff --git a/isaac_autodata_interfaces/autonomous/task_request.py b/isaac_autodata_interfaces/autonomous/task_request.py new file mode 100644 index 0000000..31e8e54 --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/task_request.py @@ -0,0 +1,411 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Strict parser for the AutoData v1 envelope around Arena environment intent.""" + +from __future__ import annotations + +import math +from pathlib import Path +from typing import Any, TypeVar + +from isaac_autodata_interfaces.autonomous._validation import ( + IssueCollector, + require_bool, + require_finite_number, + require_int, + require_mapping, + require_string, +) +from isaac_autodata_interfaces.autonomous._yaml import load_yaml_document +from isaac_autodata_interfaces.autonomous.task_request_types import ( + GenerationConfig, + MotionBackend, + OutputConfig, + PlannerBackend, + PlannerConfig, + TaskRequest, + canonical_json, +) + +TASK_REQUEST_SCHEMA_VERSION = 1 +MAX_NAME_LENGTH = 128 +MAX_PATH_LENGTH = 4096 +MAX_SUCCESSFUL_EPISODES = 1_000_000 +MAX_ATTEMPTS = 10_000_000 +MAX_NUM_ENVS = 4096 +MAX_SEED = 2**63 - 1 +MAX_PLANNER_TIME_S = 86_400.0 +MAX_BATCH_SIZE = 65_536 +MAX_INTERPOLATION_DT_S = 10.0 +MAX_OPAQUE_DEPTH = 64 +MAX_OPAQUE_NODES = 100_000 + +_EnumT = TypeVar("_EnumT", PlannerBackend, MotionBackend) + + +def load_task_request(path: str | Path) -> TaskRequest: + """Load and validate an AutoData envelope without importing Arena, Isaac, or planner code.""" + + source_path = Path(path).expanduser().resolve(strict=False) + return task_request_from_dict(load_yaml_document(source_path), source_path=source_path) + + +def task_request_from_dict(data: Any, *, source_path: str | Path) -> TaskRequest: + """Validate parsed YAML-compatible data against the exact AutoData v1 envelope.""" + + issues = IssueCollector() + root = require_mapping(data, (), issues) + if root is None: + issues.raise_if_any() + raise AssertionError("unreachable") + issues.check_keys( + root, + (), + required=("schema_version", "name", "environment", "planner", "generation", "output"), + ) + + schema_version = _parse_required_int(root, "schema_version", (), issues, minimum=1, maximum=1) + if schema_version is not None and schema_version != TASK_REQUEST_SCHEMA_VERSION: + issues.add( + ("schema_version",), + "unsupported_schema_version", + f"expected schema_version {TASK_REQUEST_SCHEMA_VERSION}, got {schema_version}", + ) + name = _parse_required_string(root, "name", (), issues) + if name is not None: + if name != name.strip(): + issues.add(("name",), "invalid_name", "name must not have leading or trailing whitespace") + if len(name) > MAX_NAME_LENGTH: + issues.add(("name",), "value_too_long", f"name must contain at most {MAX_NAME_LENGTH} characters") + + environment_intent_json = ( + _parse_environment(root["environment"], ("environment",), issues) if "environment" in root else None + ) + planner = _parse_planner(root["planner"], ("planner",), issues) if "planner" in root else None + generation = _parse_generation(root["generation"], ("generation",), issues) if "generation" in root else None + output = _parse_output(root["output"], ("output",), Path(source_path), issues) if "output" in root else None + + issues.raise_if_any() + assert schema_version is not None + assert name is not None + assert environment_intent_json is not None + assert planner is not None + assert generation is not None + assert output is not None + return TaskRequest( + schema_version=schema_version, + name=name, + environment_intent_json=environment_intent_json, + planner=planner, + generation=generation, + output=output, + source_path=Path(source_path).expanduser().resolve(strict=False), + ) + + +def _parse_environment(value: Any, path: tuple[str | int, ...], issues: IssueCollector) -> str | None: + environment = require_mapping(value, path, issues) + if environment is None: + return None + issues.check_keys(environment, path, required=("intent",)) + if "intent" not in environment: + return None + intent = require_mapping(environment["intent"], path + ("intent",), issues) + if intent is None: + return None + _validate_opaque_json(intent, path + ("intent",), issues) + if issues.issues: + return None + return canonical_json(intent) + + +def _validate_opaque_json(value: Any, path: tuple[str | int, ...], issues: IssueCollector) -> None: + """Enforce only the serialization boundary; Arena owns all nested intent semantics.""" + + stack: list[tuple[Any, tuple[str | int, ...], int]] = [(value, path, 0)] + visited = 0 + while stack: + current, current_path, depth = stack.pop() + visited += 1 + if visited > MAX_OPAQUE_NODES: + issues.add(path, "opaque_intent_too_large", f"intent exceeds {MAX_OPAQUE_NODES} values") + return + if depth > MAX_OPAQUE_DEPTH: + issues.add(current_path, "opaque_intent_too_deep", f"intent nesting exceeds {MAX_OPAQUE_DEPTH}") + continue + if current is None or type(current) in (str, bool, int): + continue + if type(current) is float: + if not math.isfinite(current): + issues.add(current_path, "non_finite", "opaque intent numbers must be finite") + continue + if type(current) is list: + for index in range(len(current) - 1, -1, -1): + stack.append((current[index], current_path + (index,), depth + 1)) + continue + if type(current) is dict: + invalid_keys = [key for key in current if type(key) is not str] + if invalid_keys: + issues.add(current_path, "invalid_mapping_key", "opaque intent mapping keys must be strings") + continue + for key in reversed(list(current)): + stack.append((current[key], current_path + (key,), depth + 1)) + continue + issues.add( + current_path, + "non_json_intent_value", + f"opaque Arena intent must contain JSON-compatible values, got {type(current).__name__}", + ) + + +def _parse_planner(value: Any, path: tuple[str | int, ...], issues: IssueCollector) -> PlannerConfig | None: + planner = require_mapping(value, path, issues) + if planner is None: + return None + fields = ( + "backend", + "motion_backend", + "collisions", + "max_time_s", + "batch_size", + "interpolation_dt_s", + "profile", + "animate", + ) + issues.check_keys(planner, path, required=fields) + backend = _parse_enum(planner, "backend", path, PlannerBackend, issues) + motion_backend = _parse_enum(planner, "motion_backend", path, MotionBackend, issues) + collisions = _parse_required_bool(planner, "collisions", path, issues) + max_time_s = _parse_required_number( + planner, + "max_time_s", + path, + issues, + minimum_exclusive=0.0, + maximum=MAX_PLANNER_TIME_S, + ) + batch_size = _parse_required_int( + planner, + "batch_size", + path, + issues, + minimum=1, + maximum=MAX_BATCH_SIZE, + ) + interpolation_dt_s = _parse_required_number( + planner, + "interpolation_dt_s", + path, + issues, + minimum_exclusive=0.0, + maximum=MAX_INTERPOLATION_DT_S, + ) + profile = _parse_required_bool(planner, "profile", path, issues) + animate = _parse_required_bool(planner, "animate", path, issues) + if None in ( + backend, + motion_backend, + collisions, + max_time_s, + batch_size, + interpolation_dt_s, + profile, + animate, + ): + return None + assert isinstance(backend, PlannerBackend) + assert isinstance(motion_backend, MotionBackend) + assert isinstance(collisions, bool) + assert isinstance(max_time_s, float) + assert isinstance(batch_size, int) + assert isinstance(interpolation_dt_s, float) + assert isinstance(profile, bool) + assert isinstance(animate, bool) + return PlannerConfig( + backend=backend, + motion_backend=motion_backend, + collisions=collisions, + max_time_s=max_time_s, + batch_size=batch_size, + interpolation_dt_s=interpolation_dt_s, + profile=profile, + animate=animate, + ) + + +def _parse_generation( + value: Any, + path: tuple[str | int, ...], + issues: IssueCollector, +) -> GenerationConfig | None: + generation = require_mapping(value, path, issues) + if generation is None: + return None + fields = ("successful_episodes", "seed", "num_envs", "max_attempts") + issues.check_keys(generation, path, required=fields) + successful_episodes = _parse_required_int( + generation, + "successful_episodes", + path, + issues, + minimum=1, + maximum=MAX_SUCCESSFUL_EPISODES, + ) + seed = _parse_required_int(generation, "seed", path, issues, minimum=0, maximum=MAX_SEED) + num_envs = _parse_required_int(generation, "num_envs", path, issues, minimum=1, maximum=MAX_NUM_ENVS) + max_attempts = _parse_required_int(generation, "max_attempts", path, issues, minimum=1, maximum=MAX_ATTEMPTS) + if successful_episodes is not None and max_attempts is not None and max_attempts < successful_episodes: + issues.add( + path + ("max_attempts",), + "attempt_budget_too_small", + f"max_attempts ({max_attempts}) must be at least successful_episodes ({successful_episodes})", + ) + if None in (successful_episodes, seed, num_envs, max_attempts): + return None + assert isinstance(successful_episodes, int) + assert isinstance(seed, int) + assert isinstance(num_envs, int) + assert isinstance(max_attempts, int) + return GenerationConfig( + successful_episodes=successful_episodes, + seed=seed, + num_envs=num_envs, + max_attempts=max_attempts, + ) + + +def _parse_output( + value: Any, + path: tuple[str | int, ...], + source_path: Path, + issues: IssueCollector, +) -> OutputConfig | None: + output = require_mapping(value, path, issues) + if output is None: + return None + issues.check_keys(output, path, required=("dataset", "keep_failed"), optional=("run_log",)) + dataset = ( + _parse_bounded_path(output["dataset"], path + ("dataset",), source_path, ".hdf5", issues) + if "dataset" in output + else None + ) + keep_failed = _parse_required_bool(output, "keep_failed", path, issues) + run_log = None + if "run_log" in output and output["run_log"] is not None: + run_log = _parse_bounded_path(output["run_log"], path + ("run_log",), source_path, ".jsonl", issues) + if dataset is None or keep_failed is None: + return None + return OutputConfig(dataset=dataset, keep_failed=keep_failed, run_log=run_log) + + +def _parse_bounded_path( + value: Any, + path: tuple[str | int, ...], + source_path: Path, + suffix: str, + issues: IssueCollector, +) -> str | None: + text = require_string(value, path, issues) + if text is None: + return None + if len(text) > MAX_PATH_LENGTH: + issues.add(path, "path_too_long", f"path must contain at most {MAX_PATH_LENGTH} characters") + return None + candidate = Path(text) + if candidate.is_absolute(): + issues.add(path, "absolute_path", "path must be relative to the request file") + return None + if candidate.parts and candidate.parts[0].startswith("~"): + issues.add(path, "home_path", "home-directory expansion is not allowed") + return None + if ".." in candidate.parts: + issues.add(path, "path_traversal", "parent-directory traversal is not allowed") + return None + if candidate.suffix.lower() != suffix: + issues.add(path, "invalid_path_suffix", f"path must end in {suffix}") + return None + request_dir = source_path.expanduser().resolve(strict=False).parent + resolved = (request_dir / candidate).resolve(strict=False) + try: + resolved.relative_to(request_dir) + except ValueError: + issues.add(path, "path_escape", "resolved path escapes the request directory") + return None + return candidate.as_posix() + + +def _parse_enum( + mapping: dict[str, Any], + key: str, + path: tuple[str | int, ...], + enum_type: type[_EnumT], + issues: IssueCollector, +) -> _EnumT | None: + value = _parse_required_string(mapping, key, path, issues) + if value is None: + return None + try: + return enum_type(value) + except ValueError: + choices = [item.value for item in enum_type] + issues.add(path + (key,), "unsupported_value", f"expected one of {choices}, got {value!r}") + return None + + +def _parse_required_string( + mapping: dict[str, Any], + key: str, + path: tuple[str | int, ...], + issues: IssueCollector, +) -> str | None: + if key not in mapping: + return None + return require_string(mapping[key], path + (key,), issues) + + +def _parse_required_bool( + mapping: dict[str, Any], + key: str, + path: tuple[str | int, ...], + issues: IssueCollector, +) -> bool | None: + if key not in mapping: + return None + return require_bool(mapping[key], path + (key,), issues) + + +def _parse_required_int( + mapping: dict[str, Any], + key: str, + path: tuple[str | int, ...], + issues: IssueCollector, + *, + minimum: int, + maximum: int, +) -> int | None: + if key not in mapping: + return None + return require_int(mapping[key], path + (key,), issues, minimum=minimum, maximum=maximum) + + +def _parse_required_number( + mapping: dict[str, Any], + key: str, + path: tuple[str | int, ...], + issues: IssueCollector, + *, + minimum_exclusive: float, + maximum: float, +) -> float | None: + if key not in mapping: + return None + return require_finite_number( + mapping[key], + path + (key,), + issues, + minimum_exclusive=minimum_exclusive, + maximum=maximum, + ) diff --git a/isaac_autodata_interfaces/autonomous/task_request_types.py b/isaac_autodata_interfaces/autonomous/task_request_types.py new file mode 100644 index 0000000..07862cd --- /dev/null +++ b/isaac_autodata_interfaces/autonomous/task_request_types.py @@ -0,0 +1,362 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Pure data contracts for the AutoData autonomous task envelope.""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import dataclass +from enum import StrEnum +from pathlib import Path +from typing import Any + + +def canonical_json(value: Any) -> str: + """Serialize a JSON-compatible value deterministically, rejecting non-finite numbers.""" + + return json.dumps(value, sort_keys=True, separators=(",", ":"), ensure_ascii=False, allow_nan=False) + + +def sha256_json(value: Any) -> str: + """Return a lowercase SHA-256 hex digest of canonical JSON content.""" + + return hashlib.sha256(canonical_json(value).encode("utf-8")).hexdigest() + + +class PlannerBackend(StrEnum): + """Task-and-motion planner selection understood by AutoData v1.""" + + AUTO = "auto" + SCHEDULESTREAM = "schedulestream" + + +class MotionBackend(StrEnum): + """Motion backend selection understood by AutoData v1.""" + + AUTO = "auto" + CUROBO_V1 = "curobo_v1" + CUROBO_V2 = "curobo_v2" + + +@dataclass(frozen=True) +class PlannerConfig: + """Validated planner and motion-backend configuration.""" + + backend: PlannerBackend + motion_backend: MotionBackend + collisions: bool + max_time_s: float + batch_size: int + interpolation_dt_s: float + profile: bool + animate: bool + + def to_dict(self) -> dict[str, Any]: + """Return canonical planner configuration.""" + + return { + "animate": self.animate, + "backend": self.backend.value, + "batch_size": self.batch_size, + "collisions": self.collisions, + "interpolation_dt_s": self.interpolation_dt_s, + "max_time_s": self.max_time_s, + "motion_backend": self.motion_backend.value, + "profile": self.profile, + } + + +@dataclass(frozen=True) +class GenerationConfig: + """Validated bounded-attempt generation configuration.""" + + successful_episodes: int + seed: int + num_envs: int + max_attempts: int + + def to_dict(self) -> dict[str, Any]: + """Return canonical generation configuration.""" + + return { + "max_attempts": self.max_attempts, + "num_envs": self.num_envs, + "seed": self.seed, + "successful_episodes": self.successful_episodes, + } + + +@dataclass(frozen=True) +class OutputConfig: + """Validated request-relative output paths before absolute resolution.""" + + dataset: str + keep_failed: bool + run_log: str | None + + def to_dict(self) -> dict[str, Any]: + """Return canonical request output configuration.""" + + return { + "dataset": self.dataset, + "keep_failed": self.keep_failed, + "run_log": self.run_log, + } + + +@dataclass(frozen=True) +class ResolvedOutputConfig: + """Output paths normalized beneath the semantic request directory.""" + + dataset: Path + keep_failed: bool + run_log: Path | None + + def to_dict(self) -> dict[str, Any]: + """Return JSON-compatible absolute output paths.""" + + return { + "dataset": str(self.dataset), + "keep_failed": self.keep_failed, + "run_log": None if self.run_log is None else str(self.run_log), + } + + +@dataclass(frozen=True) +class TaskRequest: + """Validated AutoData v1 envelope around an opaque Arena environment intent. + + Arena's nested intent is retained as canonical JSON rather than re-modelled by AutoData. + ``source_path`` is resolution context and is excluded from canonical request content. + """ + + schema_version: int + name: str + environment_intent_json: str + planner: PlannerConfig + generation: GenerationConfig + output: OutputConfig + source_path: Path + + @property + def environment_intent(self) -> dict[str, Any]: + """Return the opaque Arena intent as a fresh plain dictionary.""" + + value = json.loads(self.environment_intent_json) + assert isinstance(value, dict) + return value + + def canonical_dict(self) -> dict[str, Any]: + """Return normalized user-authored envelope content.""" + + return { + "environment": {"intent": self.environment_intent}, + "generation": self.generation.to_dict(), + "name": self.name, + "output": self.output.to_dict(), + "planner": self.planner.to_dict(), + "schema_version": self.schema_version, + } + + def canonical_json(self) -> str: + """Return deterministic canonical request JSON.""" + + return canonical_json(self.canonical_dict()) + + @property + def digest(self) -> str: + """Return the canonical envelope SHA-256 digest.""" + + return sha256_json(self.canonical_dict()) + + +@dataclass(frozen=True) +class CompilerTraceEvent: + """Plain, bounded projection of one Arena intent-resolution trace event.""" + + stage: str + query: str + chosen: str | None + note: str + + def to_dict(self) -> dict[str, str | None]: + """Return the trace event as plain data.""" + + return {"chosen": self.chosen, "note": self.note, "query": self.query, "stage": self.stage} + + +@dataclass(frozen=True) +class SpatialGoalConstraint: + """One linked Arena success-state spatial constraint, without semantic reinterpretation.""" + + id: str + kind: str + subject: str + reference: str | None + params_json: str + + @property + def params(self) -> dict[str, Any]: + """Return relation parameters as a fresh plain dictionary.""" + + value = json.loads(self.params_json) + assert isinstance(value, dict) + return value + + def to_dict(self) -> dict[str, Any]: + """Return the ordered spatial goal constraint.""" + + return { + "id": self.id, + "kind": self.kind, + "params": self.params, + "reference": self.reference, + "subject": self.subject, + } + + +@dataclass(frozen=True) +class GoalStage: + """The ordered spatial success goal for one linked Arena task.""" + + index: int + task_id: str + task_kind: str + success_state_spec_id: str + spatial_constraints: tuple[SpatialGoalConstraint, ...] + + def to_dict(self) -> dict[str, Any]: + """Return the goal stage as plain data.""" + + return { + "index": self.index, + "spatial_constraints": [constraint.to_dict() for constraint in self.spatial_constraints], + "success_state_spec_id": self.success_state_spec_id, + "task_id": self.task_id, + "task_kind": self.task_kind, + } + + +@dataclass(frozen=True) +class ArenaCompilationResult: + """Deterministic plain-data output of Arena intent validation, compilation, and linking.""" + + initial_graph_json: str + linked_graph_json: str + compiler_trace: tuple[CompilerTraceEvent, ...] + graph_digest: str + goal_stages: tuple[GoalStage, ...] + + @property + def initial_graph(self) -> dict[str, Any]: + """Return the compiled initial graph as a fresh plain dictionary.""" + + value = json.loads(self.initial_graph_json) + assert isinstance(value, dict) + return value + + @property + def linked_graph(self) -> dict[str, Any]: + """Return the linked graph as a fresh plain dictionary.""" + + value = json.loads(self.linked_graph_json) + assert isinstance(value, dict) + return value + + def to_dict(self) -> dict[str, Any]: + """Return all Arena compilation artifacts as plain data.""" + + return { + "compiler_trace": [event.to_dict() for event in self.compiler_trace], + "goal_stages": [stage.to_dict() for stage in self.goal_stages], + "graph_digest": self.graph_digest, + "initial_graph": self.initial_graph, + "linked_graph": self.linked_graph, + } + + +@dataclass(frozen=True) +class CompiledTaskRequest: + """Pure source-demo-free task ready for the autonomous runtime lane.""" + + schema_version: int + compiler_version: str + name: str + canonical_request_json: str + request_digest: str + planner: PlannerConfig + generation: GenerationConfig + output: ResolvedOutputConfig + arena: ArenaCompilationResult + source_dataset_path: None = None + + @property + def canonical_request(self) -> dict[str, Any]: + """Return canonical request content as a fresh mapping.""" + + value = json.loads(self.canonical_request_json) + assert isinstance(value, dict) + return value + + @property + def initial_graph(self) -> dict[str, Any]: + """Return the resolved Arena initial graph.""" + + return self.arena.initial_graph + + @property + def linked_graph(self) -> dict[str, Any]: + """Return the resolved Arena linked graph.""" + + return self.arena.linked_graph + + @property + def graph_digest(self) -> str: + """Return the digest of the linked Arena graph.""" + + return self.arena.graph_digest + + @property + def goal_stages(self) -> tuple[GoalStage, ...]: + """Return ordered task-success spatial goal stages.""" + + return self.arena.goal_stages + + @property + def environment_name(self) -> str: + """Return the generated Arena graph environment name.""" + + value = self.linked_graph.get("env_name") + assert isinstance(value, str) + return value + + def to_dict(self) -> dict[str, Any]: + """Return the complete compiled task as canonical-JSON-compatible data.""" + + return { + "arena": self.arena.to_dict(), + "canonical_request": self.canonical_request, + "generation": self.generation.to_dict(), + "name": self.name, + "output": self.output.to_dict(), + "planner": self.planner.to_dict(), + "request_digest": self.request_digest, + "compiler_version": self.compiler_version, + "schema_version": self.schema_version, + "source_dataset_path": self.source_dataset_path, + } + + def canonical_json(self) -> str: + """Return deterministic canonical JSON for the compiled task.""" + + return canonical_json(self.to_dict()) + + @property + def digest(self) -> str: + """Return a SHA-256 digest of the compiled task.""" + + return sha256_json(self.to_dict()) diff --git a/isaac_autodata_interfaces/embodiments/embodiment_types.py b/isaac_autodata_interfaces/embodiments/embodiment_types.py index 8b3e56d..19ddb95 100644 --- a/isaac_autodata_interfaces/embodiments/embodiment_types.py +++ b/isaac_autodata_interfaces/embodiments/embodiment_types.py @@ -15,7 +15,9 @@ class PoseObsKeys: """Observation-buffer keys for a single end-effector's pose. Both keys index into ``env.obs_buf[obs_group]``. ``pos`` returns a position - tensor; ``quat`` returns a (w, x, y, z) orientation quaternion. + tensor; ``quat`` returns an (x, y, z, w) orientation quaternion. In Isaac + Lab observation names such as ``target_quat_w``, the ``_w`` suffix denotes + the world frame; it does not denote scalar-first quaternion storage. Args: pos: Observation key for end-effector position. diff --git a/isaac_autodata_interfaces/embodiments/single_arm_embodiment_adapter.py b/isaac_autodata_interfaces/embodiments/single_arm_embodiment_adapter.py index 0c2e2aa..55f74ca 100644 --- a/isaac_autodata_interfaces/embodiments/single_arm_embodiment_adapter.py +++ b/isaac_autodata_interfaces/embodiments/single_arm_embodiment_adapter.py @@ -77,7 +77,10 @@ def get_eef_poses(self, env_ids: Sequence[int] | None = None) -> dict[str, torch assert self.env is not None, "Call bind_env(env) before reading state." index: slice | Sequence[int] = slice(None) if env_ids is None else env_ids obs = self.env.obs_buf[self.obs_group] - rot = pose_math.matrix_from_quat(obs[self.pose_obs_keys.quat][index]) + # Isaac Lab FrameTransformer observations such as ``target_quat_w`` are already XYZW; + # ``_w`` identifies the world frame, not the quaternion component order. + quaternion_xyzw = obs[self.pose_obs_keys.quat][index] + rot = pose_math.matrix_from_quat(quaternion_xyzw) pose = pose_math.make_pose(obs[self.pose_obs_keys.pos][index], rot) return {self.eef_name: self._observed_to_control_link(pose)} @@ -148,8 +151,15 @@ def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Ten assert ( action.dim() == 2 and action.shape[-1] == self.action_dim ), f"action shape must be (num_envs, {self.action_dim}), got {tuple(action.shape)}" - delta_pos = action[:, :3] - delta_aa = action[:, 3:6] + # Isaac Lab applies the DifferentialInverseKinematicsAction term's scale *after* receiving + # raw environment actions. Decode the physical delta rather than treating raw policy units + # as metres/radians. Environments without a discoverable DIK term retain the legacy unit + # scale for compatibility. + pose_action = action[:, :_DELTA_POSE_ACTION_DIM] * self._get_pose_action_scale( + num_envs=action.shape[0], dtype=action.dtype, device=action.device + ) + delta_pos = pose_action[:, :3] + delta_aa = pose_action[:, 3:6] curr_pos, curr_rot = pose_math.unmake_pose(self.get_eef_poses(env_ids=None)[self.eef_name]) target_pos = curr_pos + delta_pos delta_rot = pose_math.matrix_from_quat(pose_math.quat_from_axis_angle_vec(delta_aa)) @@ -192,6 +202,14 @@ def target_eef_pose_to_action( delta_rot = torch.matmul(target_rot, curr_rot.transpose(-1, -2)) delta_aa = pose_math.axis_angle_from_quat(pose_math.quat_from_matrix(delta_rot)) pose_action = torch.cat([delta_pos, delta_aa], dim=0) + # Encode physical deltas back into raw environment-action units. This is required for + # Franka IK-Rel, whose action term commonly uses scale=0.5. + pose_action = ( + pose_action + / self._get_pose_action_scale( + num_envs=1, dtype=pose_action.dtype, device=pose_action.device, env_id=env_id + )[0] + ) if action_noise_dict is not None: scale = action_noise_dict.get(self.eef_name, 0.0) if scale > 0.0: @@ -204,6 +222,63 @@ def target_eef_pose_to_action( ), f"gripper action must be ({self.gripper_action_dim},), got {tuple(gripper_action.shape)}" return torch.cat([pose_action, gripper_action], dim=0) + def _get_pose_action_scale( + self, + *, + num_envs: int, + dtype: torch.dtype, + device: torch.device, + env_id: int | None = None, + ) -> torch.Tensor: + """Return the live DIK action scale in pose-action order. + + Args: + num_envs: Number of rows the caller needs. + dtype: Result dtype. + device: Result device. + env_id: Optional single environment row to select. + """ + + unit = torch.ones((num_envs, _DELTA_POSE_ACTION_DIM), dtype=dtype, device=device) + if self.env is None: + return unit + action_manager = getattr(self.env, "action_manager", None) + if action_manager is None: + return unit + for term_name in getattr(action_manager, "active_terms", ()): + term = action_manager.get_term(term_name) + # Use a name/shape capability check instead of importing Isaac's action class at module + # load time. This keeps embodiment parsing usable before SimulationApp starts. + if type(term).__name__ != "DifferentialInverseKinematicsAction": + continue + scale = getattr(term, "_scale", None) + if scale is None: + scale = getattr(getattr(term, "cfg", None), "scale", None) + if scale is None: + return unit + scale_tensor = torch.as_tensor(scale, dtype=dtype, device=device) + if scale_tensor.ndim == 1: + scale_tensor = scale_tensor.unsqueeze(0) + if scale_tensor.shape[-1] != _DELTA_POSE_ACTION_DIM: + raise ValueError( + "DifferentialInverseKinematicsAction scale must have 6 pose dimensions, " + f"got shape {tuple(scale_tensor.shape)}" + ) + if torch.any(scale_tensor == 0): + raise ValueError("DifferentialInverseKinematicsAction scale must be non-zero") + if env_id is not None: + if not 0 <= env_id < scale_tensor.shape[0]: + raise ValueError(f"env_id {env_id} is outside action-scale rows {scale_tensor.shape[0]}") + scale_tensor = scale_tensor[env_id : env_id + 1] + if scale_tensor.shape[0] == 1 and num_envs > 1: + scale_tensor = scale_tensor.expand(num_envs, -1) + if scale_tensor.shape[0] != num_envs: + raise ValueError( + f"DIK action-scale rows {scale_tensor.shape[0]} do not match requested num_envs {num_envs}" + ) + return scale_tensor + return unit + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: """Slice the trailing gripper dims off a sequence of env actions. @@ -218,7 +293,8 @@ def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.T assert ( actions.shape[-1] == self.action_dim ), f"actions last dim must be {self.action_dim}, got {actions.shape[-1]}" - return {self.eef_name: actions[..., -self.gripper_action_dim :]} + gripper_start = self.action_dim - self.gripper_action_dim + return {self.eef_name: actions[..., gripper_start:]} @classmethod def from_dict(cls, data: dict[str, Any]) -> DeltaPoseIKSingleArmAdapter: diff --git a/isaac_autodata_interfaces/env/isaaclab_env_interface.py b/isaac_autodata_interfaces/env/isaaclab_env_interface.py index 6d314a4..9cb14b3 100644 --- a/isaac_autodata_interfaces/env/isaaclab_env_interface.py +++ b/isaac_autodata_interfaces/env/isaaclab_env_interface.py @@ -122,6 +122,7 @@ def env_loop( generation_policy_params: GenerationPolicy, stats: dict, data_gen_tasks: asyncio.Future | None = None, + max_attempts: int | None = None, ) -> None: """Synchronous step loop for the environment. @@ -143,17 +144,23 @@ def env_loop( stats: Shared dict with ``num_success``, ``num_failures``, and ``num_attempts`` counters. data_gen_tasks: Gathered future for all data generation tasks. When provided, the loop exits early if all tasks finish unexpectedly (e.g. due to an unhandled exception). + max_attempts: Optional hard attempt budget. Unlike ``num_trials``, this caps attempts even + when ``guarantee_success`` is true and includes failures that enqueue no action. """ - num_trials = generation_policy_params.num_trials - guarantee_success = generation_policy_params.guarantee_success env_id_tensor = torch.tensor([0], dtype=torch.int64, device=env.device) prev_num_attempts = 0 # simulate environment -- run everything in inference mode with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): while True: + if generation_stop_reason(generation_policy_params, stats, max_attempts=max_attempts) is not None: + return # check if any environment needs to be reset while waiting for actions while env_action_queue.qsize() != env.num_envs: asyncio_event_loop.run_until_complete(asyncio.sleep(0)) + # Planning can fail before producing an action. Observe attempt counters while + # waiting so these failures terminate normally instead of deadlocking here. + if generation_stop_reason(generation_policy_params, stats, max_attempts=max_attempts) is not None: + return if data_gen_tasks is not None and data_gen_tasks.done(): exc = data_gen_tasks.exception() if exc is not None: @@ -189,11 +196,15 @@ def env_loop( print(f"{num_success}/{num_attempts} ({generated_success_rate:.1f}%) successful demos generated\033[K") print("*" * 50, "\033[K") - # termination condition is on enough successes if guarantee_success else enough attempts - check_val = num_success if guarantee_success else num_attempts - if check_val >= num_trials: - print(f"Reached {num_trials} {'successes' if guarantee_success else 'attempts'}. Exiting.") - break + # Stop on the requested successes/attempts or the independent hard budget. + stop_reason = generation_stop_reason( + generation_policy_params, + stats, + max_attempts=max_attempts, + ) + if stop_reason is not None: + print(f"Generation stop condition reached ({stop_reason}). Exiting.") + return # check that simulation is stopped or not if env.sim.is_stopped(): @@ -201,3 +212,35 @@ def env_loop( # Do not close env here: async data generator tasks may still be running. # Caller must close env after cancelling and awaiting those tasks. + + +def generation_stop_reason( + generation_policy_params: GenerationPolicy, + stats: dict, + *, + max_attempts: int | None = None, +) -> str | None: + """Return the reached generation stop condition, or ``None``. + + Args: + generation_policy_params: Desired success/attempt target. + stats: Current ``num_success``, ``num_failures``, and ``num_attempts`` counters. + max_attempts: Optional hard cap applied independently of ``guarantee_success``. + """ + + for name in ("num_success", "num_failures", "num_attempts"): + assert name in stats, f"generation stats missing {name!r}" + assert isinstance(stats[name], int) and stats[name] >= 0, f"generation stat {name!r} must be non-negative" + assert ( + stats["num_success"] + stats["num_failures"] == stats["num_attempts"] + ), "generation successes and failures must sum to attempts" + if max_attempts is not None: + assert isinstance(max_attempts, int) and max_attempts > 0, "max_attempts must be positive" + + target_value = stats["num_success"] if generation_policy_params.guarantee_success else stats["num_attempts"] + if target_value >= generation_policy_params.num_trials: + target_name = "successes" if generation_policy_params.guarantee_success else "attempts" + return f"requested_{target_name}={generation_policy_params.num_trials}" + if max_attempts is not None and stats["num_attempts"] >= max_attempts: + return f"max_attempts={max_attempts}" + return None diff --git a/isaac_autodata_interfaces/motion_planners/__init__.py b/isaac_autodata_interfaces/motion_planners/__init__.py index 1dd1a68..42469f1 100644 --- a/isaac_autodata_interfaces/motion_planners/__init__.py +++ b/isaac_autodata_interfaces/motion_planners/__init__.py @@ -17,11 +17,10 @@ from typing import TYPE_CHECKING -from isaac_autodata_interfaces.motion_planners.curobo import CuroboPlannerCfg from isaac_autodata_interfaces.motion_planners.motion_planner_base import MotionPlannerBase if TYPE_CHECKING: - from isaac_autodata_interfaces.motion_planners.curobo import CuroboPlanner + from isaac_autodata_interfaces.motion_planners.curobo import CuroboPlanner, CuroboPlannerCfg __all__ = [ "MotionPlannerBase", @@ -35,4 +34,8 @@ def __getattr__(name: str): from isaac_autodata_interfaces.motion_planners.curobo import CuroboPlanner as _CuroboPlanner return _CuroboPlanner + if name == "CuroboPlannerCfg": + from isaac_autodata_interfaces.motion_planners.curobo import CuroboPlannerCfg as _CuroboPlannerCfg + + return _CuroboPlannerCfg raise AttributeError(f"module 'motion_planners' has no attribute {name!r}") diff --git a/isaac_autodata_interfaces/motion_planners/curobo/__init__.py b/isaac_autodata_interfaces/motion_planners/curobo/__init__.py index d4666f9..61dfb54 100644 --- a/isaac_autodata_interfaces/motion_planners/curobo/__init__.py +++ b/isaac_autodata_interfaces/motion_planners/curobo/__init__.py @@ -15,8 +15,7 @@ if TYPE_CHECKING: from isaac_autodata_interfaces.motion_planners.curobo.curobo_planner import CuroboPlanner - -from isaac_autodata_interfaces.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg + from isaac_autodata_interfaces.motion_planners.curobo.curobo_planner_cfg import CuroboPlannerCfg __all__ = [ "CuroboPlanner", @@ -29,4 +28,10 @@ def __getattr__(name: str): from isaac_autodata_interfaces.motion_planners.curobo.curobo_planner import CuroboPlanner as _CuroboPlanner return _CuroboPlanner + if name == "CuroboPlannerCfg": + from isaac_autodata_interfaces.motion_planners.curobo.curobo_planner_cfg import ( + CuroboPlannerCfg as _CuroboPlannerCfg, + ) + + return _CuroboPlannerCfg raise AttributeError(f"module 'curobo' has no attribute {name!r}") diff --git a/isaac_autodata_interfaces/motion_planners/curobo/backend_selection.py b/isaac_autodata_interfaces/motion_planners/curobo/backend_selection.py new file mode 100644 index 0000000..48426bf --- /dev/null +++ b/isaac_autodata_interfaces/motion_planners/curobo/backend_selection.py @@ -0,0 +1,312 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Import-free cuRobo detection and ScheduleStream backend selection. + +ScheduleStream has two distinct manipulation applications: ``custream`` targets the cuRobo v1 +module layout, while ``custream2`` targets cuRobo v2. This module detects concrete API markers on +disk without importing either stack, then makes backend selection explicit and reproducible. +""" + +from __future__ import annotations + +import importlib.machinery +import importlib.metadata +import importlib.util +import json +import re +from collections.abc import Callable +from dataclasses import dataclass +from enum import StrEnum +from pathlib import Path +from typing import Any +from urllib.parse import urlsplit, urlunsplit + + +class CuroboApiGeneration(StrEnum): + """Detected public API family.""" + + UNAVAILABLE = "unavailable" + V1 = "v1" + V2 = "v2" + UNKNOWN = "unknown" + + +CUROBO_V1_MARKERS = ( + "curobo.wrap.reacher.motion_gen", + "curobo.types.math", + "curobo.types.state", + "curobo.util.usd_helper", +) +CUROBO_V2_MARKERS = ( + "curobo.motion_planner", + "curobo.scene", + "curobo.types", + "curobo._src.util.usd_scene_parser", +) +SCHEDULESTREAM_V1_MARKERS = ( + "schedulestream.applications.custream.example", + "schedulestream.applications.custream.world", +) +SCHEDULESTREAM_V2_MARKERS = ( + "schedulestream.applications.custream2.policy", + "schedulestream.applications.custream2.world", +) + + +@dataclass(frozen=True) +class DistributionIdentity: + """Installed distribution version and optional direct-source commit.""" + + name: str + version: str + source_url: str | None = None + source_commit: str | None = None + + def to_dict(self) -> dict[str, str | None]: + return { + "name": self.name, + "source_commit": self.source_commit, + "source_url": self.source_url, + "version": self.version, + } + + +@dataclass(frozen=True) +class CuroboRuntimeCapabilities: + """Concrete motion/planning capabilities present in one Python runtime.""" + + api_generation: CuroboApiGeneration + curobo: DistributionIdentity | None + schedulestream: DistributionIdentity | None + has_schedulestream_v1: bool + has_schedulestream_v2: bool + missing_v1_markers: tuple[str, ...] + missing_v2_markers: tuple[str, ...] + missing_schedulestream_v1_markers: tuple[str, ...] + missing_schedulestream_v2_markers: tuple[str, ...] + + @property + def motion_backend_id(self) -> str | None: + if self.api_generation is CuroboApiGeneration.V1: + return "curobo_v1" + if self.api_generation is CuroboApiGeneration.V2: + return "curobo_v2" + return None + + @property + def schedulestream_application(self) -> str | None: + if self.api_generation is CuroboApiGeneration.V1 and self.has_schedulestream_v1: + return "custream" + if self.api_generation is CuroboApiGeneration.V2 and self.has_schedulestream_v2: + return "custream2" + return None + + @property + def supports_schedulestream(self) -> bool: + return self.schedulestream_application is not None + + def to_dict(self) -> dict[str, Any]: + return { + "api_generation": self.api_generation.value, + "curobo": None if self.curobo is None else self.curobo.to_dict(), + "has_schedulestream_v1": self.has_schedulestream_v1, + "has_schedulestream_v2": self.has_schedulestream_v2, + "missing_markers": { + "curobo_v1": list(self.missing_v1_markers), + "curobo_v2": list(self.missing_v2_markers), + "schedulestream_v1": list(self.missing_schedulestream_v1_markers), + "schedulestream_v2": list(self.missing_schedulestream_v2_markers), + }, + "motion_backend_id": self.motion_backend_id, + "schedulestream": None if self.schedulestream is None else self.schedulestream.to_dict(), + "schedulestream_application": self.schedulestream_application, + "supports_schedulestream": self.supports_schedulestream, + } + + +@dataclass(frozen=True) +class MotionBackendSelection: + """Resolved cuRobo/ScheduleStream implementation for one run.""" + + motion_backend: str + schedulestream_application: str + capabilities: CuroboRuntimeCapabilities + + +class BackendCompatibilityError(RuntimeError): + """Raised when requested planner and installed API capabilities do not agree.""" + + +def detect_curobo_runtime( + *, + module_available: Callable[[str], bool] | None = None, + distribution_reader: Callable[[tuple[str, ...]], DistributionIdentity | None] | None = None, +) -> CuroboRuntimeCapabilities: + """Inspect cuRobo/ScheduleStream API markers without importing their packages. + + Args: + module_available: Optional test hook returning whether a module path exists. + distribution_reader: Optional test hook resolving installed distribution identity. + """ + + module_available = module_available or _module_available_without_import + distribution_reader = distribution_reader or _read_distribution_identity + + curobo_root = module_available("curobo") + missing_v1 = tuple(marker for marker in CUROBO_V1_MARKERS if not module_available(marker)) + missing_v2 = tuple(marker for marker in CUROBO_V2_MARKERS if not module_available(marker)) + has_v1 = curobo_root and not missing_v1 + has_v2 = curobo_root and not missing_v2 + if not curobo_root: + api_generation = CuroboApiGeneration.UNAVAILABLE + elif has_v2: + # A v2 install may retain compatibility modules with v1 names. Prefer its defining v2 API. + api_generation = CuroboApiGeneration.V2 + elif has_v1: + api_generation = CuroboApiGeneration.V1 + else: + api_generation = CuroboApiGeneration.UNKNOWN + + schedulestream_root = module_available("schedulestream") + missing_schedule_v1 = tuple(marker for marker in SCHEDULESTREAM_V1_MARKERS if not module_available(marker)) + missing_schedule_v2 = tuple(marker for marker in SCHEDULESTREAM_V2_MARKERS if not module_available(marker)) + return CuroboRuntimeCapabilities( + api_generation=api_generation, + curobo=distribution_reader(("nvidia-curobo", "curobo")) if curobo_root else None, + schedulestream=(distribution_reader(("schedulestream",)) if schedulestream_root else None), + has_schedulestream_v1=schedulestream_root and not missing_schedule_v1, + has_schedulestream_v2=schedulestream_root and not missing_schedule_v2, + missing_v1_markers=missing_v1, + missing_v2_markers=missing_v2, + missing_schedulestream_v1_markers=missing_schedule_v1, + missing_schedulestream_v2_markers=missing_schedule_v2, + ) + + +def select_schedulestream_backend( + requested_motion_backend: str, + capabilities: CuroboRuntimeCapabilities | None = None, +) -> MotionBackendSelection: + """Select the ScheduleStream application compatible with the installed cuRobo API. + + Args: + requested_motion_backend: ``auto``, ``curobo_v1``, or ``curobo_v2``. + capabilities: Precomputed capabilities; detected from the current runtime when omitted. + """ + + if requested_motion_backend not in ("auto", "curobo_v1", "curobo_v2"): + raise BackendCompatibilityError( + f"motion_backend must be one of ['auto', 'curobo_v1', 'curobo_v2'], got {requested_motion_backend!r}" + ) + capabilities = capabilities or detect_curobo_runtime() + detected = capabilities.motion_backend_id + if detected is None: + raise BackendCompatibilityError( + "No supported cuRobo API was detected. " + f"Detected state: {capabilities.api_generation.value}; " + f"missing v1 markers: {list(capabilities.missing_v1_markers)}; " + f"missing v2 markers: {list(capabilities.missing_v2_markers)}." + ) + if requested_motion_backend != "auto" and requested_motion_backend != detected: + raise BackendCompatibilityError( + f"Requested {requested_motion_backend}, but this runtime provides {detected}. " + "Use motion_backend: auto or select a matching runtime profile." + ) + application = capabilities.schedulestream_application + if application is None: + expected_markers = ( + capabilities.missing_schedulestream_v1_markers + if detected == "curobo_v1" + else capabilities.missing_schedulestream_v2_markers + ) + expected_application = "custream" if detected == "curobo_v1" else "custream2" + raise BackendCompatibilityError( + f"cuRobo backend {detected} is available, but ScheduleStream's {expected_application} " + f"application is not complete; missing markers: {list(expected_markers)}. " + "Install the reviewed pinned ScheduleStream runtime in its development image; " + "the AutoData host environment will not be modified automatically." + ) + return MotionBackendSelection( + motion_backend=detected, + schedulestream_application=application, + capabilities=capabilities, + ) + + +def _module_available_without_import(module_name: str) -> bool: + """Return whether ``module_name`` exists on disk without importing a parent package.""" + + parts = module_name.split(".") + try: + root_spec = importlib.util.find_spec(parts[0]) + except (ImportError, ModuleNotFoundError, ValueError): + return False + if root_spec is None: + return False + if len(parts) == 1: + return True + search_locations = root_spec.submodule_search_locations + if search_locations is None: + return False + candidates = [Path(location).joinpath(*parts[1:]) for location in search_locations] + suffixes = tuple(importlib.machinery.all_suffixes()) + return any( + candidate.is_dir() or any(Path(f"{candidate}{suffix}").is_file() for suffix in suffixes) + for candidate in candidates + ) + + +def _read_distribution_identity(names: tuple[str, ...]) -> DistributionIdentity | None: + for name in names: + try: + distribution = importlib.metadata.distribution(name) + except importlib.metadata.PackageNotFoundError: + continue + source_url = None + source_commit = None + direct_url_text = distribution.read_text("direct_url.json") + if direct_url_text: + try: + direct_url = json.loads(direct_url_text) + except json.JSONDecodeError: + direct_url = {} + if isinstance(direct_url, dict): + raw_url = direct_url.get("url") + if isinstance(raw_url, str): + source_url = _sanitize_source_url(raw_url) + vcs_info = direct_url.get("vcs_info") + if isinstance(vcs_info, dict): + raw_commit = vcs_info.get("commit_id") + if isinstance(raw_commit, str) and re.fullmatch(r"[0-9a-fA-F]{7,128}", raw_commit): + source_commit = raw_commit + return DistributionIdentity( + name=distribution.metadata.get("Name", name), + version=distribution.version, + source_url=source_url, + source_commit=source_commit, + ) + return None + + +def _sanitize_source_url(raw_url: str) -> str | None: + """Remove credentials and request data before an install URL enters runtime identity records.""" + + if not raw_url or len(raw_url) > 8192 or any(character in raw_url for character in "\r\n\x00"): + return None + try: + parsed = urlsplit(raw_url) + if parsed.scheme.lower() == "file": + return "file:///" + hostname = parsed.hostname + if not parsed.scheme or hostname is None: + return None + host = f"[{hostname}]" if ":" in hostname else hostname + if parsed.port is not None: + host = f"{host}:{parsed.port}" + sanitized = urlunsplit((parsed.scheme.lower(), host, parsed.path, "", "")) + except ValueError: + return None + return sanitized[:2048] diff --git a/isaac_autodata_interfaces/tasks/subtask_spec.py b/isaac_autodata_interfaces/tasks/subtask_spec.py index 63a85c3..c52eb29 100644 --- a/isaac_autodata_interfaces/tasks/subtask_spec.py +++ b/isaac_autodata_interfaces/tasks/subtask_spec.py @@ -34,6 +34,30 @@ class DexMimicGenSubtaskAlgoParams(SubtaskAlgoParams): pass +@dataclass +class ScheduleStreamSubtaskAlgoParams(SubtaskAlgoParams): + """ScheduleStream solver/debug config. + + ScheduleStream plans the whole task in one shot from its single subtask, so these task-wide + knobs live on that subtask's ``algo_params`` (read via ``get_subtask_algo_params``) instead of + cluttering the shared CLI / :class:`GenerationPolicy`. Set them under ``algo_params:`` in the + task descriptor's subtask. + + Args: + collisions: Plan with collision checking enabled. + max_time: ``solve_tamp`` wall-clock budget, in seconds. + profile: Profile world creation and ``solve_tamp`` separately. + hold: If not ``None``, skip TAMP and hold the current configuration for this many steps. + animate: Animate the plan in the cuStream2 viewer before executing (blocking). + """ + + collisions: bool = True + max_time: float = 60.0 + profile: bool = False + hold: int | None = None + animate: bool = False + + @dataclass class SkillGenSubtaskAlgoParams(SubtaskAlgoParams): """SkillGen-specific subtask parameters. @@ -96,6 +120,7 @@ class Subtask: "mimicgen": MimicGenSubtaskAlgoParams, "dexmimicgen": DexMimicGenSubtaskAlgoParams, "skillgen": SkillGenSubtaskAlgoParams, + "schedulestream": ScheduleStreamSubtaskAlgoParams, } """Maps the ``algo:`` discriminator in a YAML task config to the corresponding :class:`SubtaskAlgoParams` subclass. diff --git a/isaac_autodata_tests/core/__init__.py b/isaac_autodata_tests/core/__init__.py new file mode 100644 index 0000000..58bafe8 --- /dev/null +++ b/isaac_autodata_tests/core/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Core test package.""" diff --git a/isaac_autodata_tests/core/autonomous/__init__.py b/isaac_autodata_tests/core/autonomous/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/core/autonomous/test_attempt_generation.py b/isaac_autodata_tests/core/autonomous/test_attempt_generation.py new file mode 100644 index 0000000..4a66997 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_attempt_generation.py @@ -0,0 +1,436 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import asyncio +import json +from dataclasses import replace + +import pytest + +from isaac_autodata_core.autonomous.attempt_generation import ( + AttemptGenerationError, + AttemptGenerator, + AttemptRequest, + ExecutionResult, + FailureStage, +) +from isaac_autodata_core.autonomous.run_log import RunLogWriter, RunLogWriteUncertainError +from isaac_autodata_core.autonomous.task_motion import ( + ExecutionEvent, + ExecutionEventType, + ExecutionOutcome, + GoalPredicate, + RobotStateSnapshot, + SceneSnapshot, + TaskMotionPlan, + WaitSegment, +) + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +class _Runtime: + def __init__(self) -> None: + self.finished: list[tuple[bool, bool]] = [] + self.snapshot = SceneSnapshot( + snapshot_id="placeholder", + captured_at_s=0.0, + env_id=0, + robot=RobotStateSnapshot( + robot_id="robot", + joint_names=("joint",), + joint_positions=(0.0,), + eef_poses={"eef": IDENTITY}, + ), + objects=(), + ) + + async def reset_attempt(self, env_id: int): + return {"env_id": env_id} + + def capture_scene_snapshot(self, env_id: int, *, snapshot_id: str): + return SceneSnapshot( + snapshot_id=snapshot_id, + captured_at_s=0.0, + env_id=env_id, + robot=self.snapshot.robot, + objects=(), + ) + + async def finish_attempt(self, env_id: int, *, success: bool, keep_failed: bool): + self.finished.append((success, keep_failed)) + + +class _CancellingRuntime(_Runtime): + async def reset_attempt(self, env_id: int): + raise asyncio.CancelledError + + +class _ResetEvidenceRuntime(_Runtime): + def __init__(self, reset_evidence) -> None: + super().__init__() + self.reset_evidence = reset_evidence + + async def reset_attempt(self, env_id: int): + del env_id + return self.reset_evidence + + +class _SlowFinalizingCancellingRuntime(_CancellingRuntime): + def __init__(self) -> None: + super().__init__() + self.finalizer_started = asyncio.Event() + self.release_finalizer = asyncio.Event() + + async def finish_attempt(self, env_id: int, *, success: bool, keep_failed: bool): + self.finalizer_started.set() + await self.release_finalizer.wait() + await super().finish_attempt(env_id, success=success, keep_failed=keep_failed) + + +class _Planner: + def __init__(self, *, fail: bool = False) -> None: + self.fail = fail + self.closed = False + + def plan(self, request, snapshot): + if self.fail: + raise AttemptGenerationError(FailureStage.PLANNING, "no_plan", "no feasible plan") + return TaskMotionPlan( + plan_id="plan", + request_digest=request.request_digest, + snapshot_digest=snapshot.digest, + backend="fake", + backend_version="1", + seed=request.seed, + segments=(WaitSegment(segment_id="wait", steps=1),), + goal=request.goal, + ) + + def close(self): + self.closed = True + + +class _Executor: + def __init__(self, *, success: bool = True, recoverable: bool = True) -> None: + self.success = success + self.recoverable = recoverable + self.calls = 0 + + async def execute(self, request, plan): + self.calls += 1 + event = ExecutionEvent( + event_id="event", + attempt_id=request.attempt_id, + plan_id=plan.plan_id, + segment_id=None, + event_type=(ExecutionEventType.TASK_VERIFIED if self.success else ExecutionEventType.TASK_REJECTED), + outcome=(ExecutionOutcome.SUCCEEDED if self.success else ExecutionOutcome.FAILED), + monotonic_time_s=0.1, + ) + return ExecutionResult( + success=self.success, + events=(event,), + failure_code=None if self.success else "task_not_stable", + recoverable=self.recoverable, + ) + + +class _TamperedPlanner(_Planner): + def __init__(self, **changes) -> None: + super().__init__() + self.changes = changes + + def plan(self, request, snapshot): + return replace(super().plan(request, snapshot), **self.changes) + + +class _FailingWriter: + def append(self, _record): + raise OSError("ledger unavailable") + + +class _AmbiguousWriter: + def __init__(self, fail_on: str) -> None: + self.fail_on = fail_on + self.record_types: list[str] = [] + + def append(self, record): + record_type = record["record_type"] + self.record_types.append(record_type) + if record_type == self.fail_on: + raise RunLogWriteUncertainError("ledger tail is ambiguous") + + +def _request() -> AttemptRequest: + return AttemptRequest( + request_digest="request", + attempt_index=0, + seed=7, + env_id=0, + goal=(GoalPredicate("on", "cube", "table"),), + keep_failed=True, + ) + + +def test_successful_attempt_finishes_recorder_and_writes_run_log(tmp_path): + runtime = _Runtime() + planner = _Planner() + executor = _Executor() + path = tmp_path / "attempts.jsonl" + generator = AttemptGenerator( + runtime, + planner, + executor, + run_log_writer=RunLogWriter(path), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert result.success + assert runtime.finished == [(True, True)] + assert executor.calls == 1 + records = [json.loads(line) for line in path.read_text(encoding="utf-8").splitlines()] + assert len(records) == 2 + assert records[0]["record_type"] == "attempt_prepared" + assert records[0]["reset_evidence"] == {"env_id": 0} + assert records[1]["record_type"] == "attempt_recorded" + + +def test_prepared_run_log_preserves_bounded_reset_settling_evidence(tmp_path): + reset_evidence = { + "env_id": 0, + "recorder_excludes_reset_settling": True, + "reset_completed": True, + "reset_settle_duration_s": 0.2, + "reset_settle_steps": 10, + } + runtime = _ResetEvidenceRuntime(reset_evidence) + path = tmp_path / "attempts.jsonl" + generator = AttemptGenerator( + runtime, + _Planner(), + _Executor(), + run_log_writer=RunLogWriter(path), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + prepared = json.loads(path.read_text(encoding="utf-8").splitlines()[0]) + assert result.success + assert prepared["record_type"] == "attempt_prepared" + assert prepared["reset_evidence"] == reset_evidence + + +@pytest.mark.parametrize( + "reset_evidence", + [ + {1: "non-string key"}, + {"unsupported": object()}, + ["not", "a", "mapping"], + ], +) +def test_malformed_reset_evidence_fails_run_log_before_dataset_export(tmp_path, reset_evidence): + runtime = _ResetEvidenceRuntime(reset_evidence) + path = tmp_path / "attempts.jsonl" + generator = AttemptGenerator( + runtime, + _Planner(), + _Executor(), + run_log_writer=RunLogWriter(path), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.RECORDING + assert result.failure_code == "run_log_prepare_failed" + assert runtime.finished == [(False, True)] + assert path.read_bytes() == b"" + + +def test_oversized_reset_evidence_fails_run_log_before_dataset_export(tmp_path): + runtime = _ResetEvidenceRuntime({"payload": "x" * 70_000}) + path = tmp_path / "attempts.jsonl" + generator = AttemptGenerator( + runtime, + _Planner(), + _Executor(), + run_log_writer=RunLogWriter(path), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.RECORDING + assert result.failure_code == "run_log_prepare_failed" + assert runtime.finished == [(False, True)] + assert path.read_bytes() == b"" + + +def test_no_action_planning_failure_is_a_completed_failed_attempt(): + runtime = _Runtime() + executor = _Executor() + generator = AttemptGenerator(runtime, _Planner(fail=True), executor) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.PLANNING + assert result.failure_code == "no_plan" + assert executor.calls == 0 + assert runtime.finished == [(False, True)] + + +def test_final_verification_failure_cannot_be_labeled_successful(): + runtime = _Runtime() + generator = AttemptGenerator(runtime, _Planner(), _Executor(success=False)) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "task_not_stable" + assert runtime.finished == [(False, True)] + + +def test_executor_nonrecoverable_failure_survives_attempt_classification(): + runtime = _Runtime() + generator = AttemptGenerator( + runtime, + _Planner(), + _Executor(success=False, recoverable=False), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "task_not_stable" + assert not result.recoverable + assert runtime.finished == [(False, True)] + + +def test_close_releases_planner_resources(): + planner = _Planner() + generator = AttemptGenerator(_Runtime(), planner, _Executor()) + + generator.close() + + assert planner.closed + + +@pytest.mark.parametrize( + ("changes", "expected_code"), + [ + ({"seed": 8}, "seed_mismatch"), + ({"goal": (GoalPredicate("in", "cube", "bowl"),)}, "goal_mismatch"), + ({"backend": "unexpected"}, "plan_backend_mismatch"), + ], +) +def test_plan_attestation_rejects_tampering_before_execution(changes, expected_code): + runtime = _Runtime() + executor = _Executor() + generator = AttemptGenerator(runtime, _TamperedPlanner(**changes), executor) + request = replace(_request(), expected_plan_backend="fake") + + result = asyncio.run(generator.generate_attempt(request)) + + assert not result.success + assert result.failure_stage is FailureStage.LOWERING + assert result.failure_code == expected_code + assert not result.recoverable + assert executor.calls == 0 + assert runtime.finished == [(False, True)] + + +def test_cancellation_finalizes_attempt_as_failed_before_propagating(): + runtime = _CancellingRuntime() + generator = AttemptGenerator(runtime, _Planner(), _Executor()) + + with pytest.raises(asyncio.CancelledError): + asyncio.run(generator.generate_attempt(_request())) + + assert runtime.finished == [(False, True)] + + +def test_cancellation_without_running_loop_finalizes_inline_before_propagating(): + runtime = _CancellingRuntime() + generator = AttemptGenerator(runtime, _Planner(), _Executor()) + operation = generator.generate_attempt(_request()) + + with pytest.raises(asyncio.CancelledError): + operation.send(None) + + assert runtime.finished == [(False, True)] + assert operation.cr_frame is None + + +def test_repeated_cancellation_does_not_detach_attempt_finalizer(): + async def run_scenario(): + runtime = _SlowFinalizingCancellingRuntime() + generator = AttemptGenerator(runtime, _Planner(), _Executor()) + task = asyncio.create_task(generator.generate_attempt(_request())) + await runtime.finalizer_started.wait() + task.cancel() + await asyncio.sleep(0) + task.cancel() + runtime.release_finalizer.set() + with pytest.raises(asyncio.CancelledError): + await task + return runtime + + runtime = asyncio.run(run_scenario()) + + assert runtime.finished == [(False, True)] + + +def test_run_log_prepare_failure_prevents_successful_dataset_export(): + runtime = _Runtime() + generator = AttemptGenerator( + runtime, + _Planner(), + _Executor(), + run_log_writer=_FailingWriter(), + ) + + result = asyncio.run(generator.generate_attempt(_request())) + + assert not result.success + assert result.failure_stage is FailureStage.RECORDING + assert result.failure_code == "run_log_prepare_failed" + assert not result.recoverable + assert runtime.finished == [(False, True)] + + +def test_ambiguous_prepare_append_finalizes_failure_then_propagates() -> None: + runtime = _Runtime() + writer = _AmbiguousWriter("attempt_prepared") + generator = AttemptGenerator(runtime, _Planner(), _Executor(), run_log_writer=writer) + + with pytest.raises(RunLogWriteUncertainError, match="ambiguous"): + asyncio.run(generator.generate_attempt(_request())) + + assert writer.record_types == ["attempt_prepared"] + assert runtime.finished == [(False, True)] + + +def test_ambiguous_commit_append_propagates_without_later_ledger_write() -> None: + runtime = _Runtime() + writer = _AmbiguousWriter("attempt_recorded") + generator = AttemptGenerator(runtime, _Planner(), _Executor(), run_log_writer=writer) + + with pytest.raises(RunLogWriteUncertainError, match="ambiguous"): + asyncio.run(generator.generate_attempt(_request())) + + assert writer.record_types == ["attempt_prepared", "attempt_recorded"] + assert runtime.finished == [(True, True)] diff --git a/isaac_autodata_tests/core/autonomous/test_dataset_generation.py b/isaac_autodata_tests/core/autonomous/test_dataset_generation.py new file mode 100644 index 0000000..d9fc9f6 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_dataset_generation.py @@ -0,0 +1,121 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import asyncio + +import pytest + +from isaac_autodata_core.autonomous.attempt_generation import AttemptResult, FailureStage +from isaac_autodata_core.autonomous.dataset_generation import ( + MAX_ATTEMPT_SEED, + DatasetGenerationRequest, + generate_dataset, +) +from isaac_autodata_core.autonomous.task_motion import GoalPredicate + + +class _FakeGenerator: + def __init__(self, outcomes: list[tuple[bool, bool]]) -> None: + self.outcomes = outcomes + self.requests = [] + self.closed = False + + async def generate_attempt(self, request): + self.requests.append(request) + success, recoverable = self.outcomes[len(self.requests) - 1] + return AttemptResult( + attempt_id=request.attempt_id, + success=success, + initial_state={}, + snapshot=None, + plan=None, + execution=None, + failure_stage=None if success else FailureStage.PLANNING, + failure_code=None if success else "no_plan", + failure_message=None if success else "no plan", + recoverable=recoverable, + ) + + def close(self): + self.closed = True + + +def _request(**overrides): + values = { + "request_digest": "abc", + "goal": (GoalPredicate("on", "cube", "table"),), + "successful_episodes": 2, + "max_attempts": 5, + "base_seed": 11, + "num_envs": 2, + "keep_failed": False, + } + values.update(overrides) + return DatasetGenerationRequest(**values) + + +def test_failed_attempts_do_not_count_toward_success_target(): + generator = _FakeGenerator([(False, True), (True, True), (True, True)]) + + summary = asyncio.run(generate_dataset(generator, _request())) + + assert summary.target_reached + assert (summary.attempts, summary.successes, summary.failures) == (3, 2, 1) + assert summary.stop_reason == "requested_successes" + assert [request.seed for request in generator.requests] == [11, 12, 13] + assert [request.env_id for request in generator.requests] == [0, 1, 0] + assert generator.closed + + +def test_hard_attempt_budget_is_terminal(): + generator = _FakeGenerator([(False, True), (False, True), (False, True)]) + + summary = asyncio.run(generate_dataset(generator, _request(successful_episodes=2, max_attempts=3))) + + assert not summary.target_reached + assert summary.stop_reason == "max_attempts" + assert summary.attempts == 3 + + +def test_unrecoverable_failure_stops_immediately(): + generator = _FakeGenerator([(False, False), (True, True), (True, True)]) + + summary = asyncio.run(generate_dataset(generator, _request())) + + assert not summary.target_reached + assert summary.stop_reason == "unrecoverable_failure" + assert summary.attempts == 1 + + +def test_close_can_be_owned_by_caller(): + generator = _FakeGenerator([(True, True), (True, True)]) + + asyncio.run(generate_dataset(generator, _request(), close=False)) + + assert not generator.closed + + +def test_attempt_seed_wraps_within_signed_int64_contract(): + generator = _FakeGenerator([(False, True), (True, True), (True, True)]) + + asyncio.run(generate_dataset(generator, _request(base_seed=MAX_ATTEMPT_SEED))) + + assert [request.seed for request in generator.requests] == [MAX_ATTEMPT_SEED, 0, 1] + + +@pytest.mark.parametrize( + ("overrides", "message"), + [ + ({"successful_episodes": True}, "successful_episodes must be an integer"), + ({"base_seed": MAX_ATTEMPT_SEED + 1}, "base_seed must be in"), + ({"keep_failed": 1}, "keep_failed must be a boolean"), + ({"expected_plan_backend": ""}, "expected_plan_backend"), + ], +) +def test_generation_request_rejects_invalid_public_contracts(overrides, message): + with pytest.raises(ValueError, match=message): + _request(**overrides) diff --git a/isaac_autodata_tests/core/autonomous/test_dense_trace.py b/isaac_autodata_tests/core/autonomous/test_dense_trace.py new file mode 100644 index 0000000..2797439 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_dense_trace.py @@ -0,0 +1,133 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import pytest + +from isaac_autodata_core.autonomous.dense_trace import ( + DenseAttachmentEvent, + DensePlanTrace, + task_motion_plan_from_dense_trace, +) +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, +) + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +def test_dense_trace_splits_gripper_changes_and_preserves_joint_seeds(): + trace = DensePlanTrace( + eef_name="franka", + frame="robot_base", + poses=(IDENTITY, IDENTITY, IDENTITY, IDENTITY), + gripper_values=(1.0, 1.0, -1.0, -1.0), + step_dt_s=0.05, + joint_names=("j1", "j2"), + joint_positions=((0.0, 0.0), (0.1, 0.0), (0.1, 0.0), (0.3, 0.0)), + attachment_events=(DenseAttachmentEvent(2, "attach", "cube"),), + ) + + plan = task_motion_plan_from_dense_trace( + trace, + request_digest="request", + snapshot_digest="snapshot", + backend="schedulestream_custream", + backend_version="test", + seed=0, + goal=(GoalPredicate("on", "cube", "table"),), + ) + + gripper_segments = [segment for segment in plan.segments if isinstance(segment, GripperCommandSegment)] + cartesian_segments = [segment for segment in plan.segments if isinstance(segment, CartesianTrajectorySegment)] + attach_segments = [segment for segment in plan.segments if isinstance(segment, AttachIntentSegment)] + assert [segment.command for segment in gripper_segments] == [ + GripperCommandMode.OPEN, + GripperCommandMode.CLOSE, + ] + assert len(cartesian_segments) == 2 + assert [segment.duration_s for segment in cartesian_segments] == [0.1, 0.1] + assert cartesian_segments[0].joint_seed_names == ("j1", "j2") + assert cartesian_segments[1].joint_seeds[-1] == (0.3, 0.0) + assert attach_segments[0].object_name == "cube" + assert all( + not segment.depends_on or segment.depends_on[0] == plan.segments[index - 1].segment_id + for index, segment in enumerate(plan.segments) + ) + + +def test_continuous_gripper_value_becomes_position_command(): + trace = DensePlanTrace( + eef_name="hand", + frame="base", + poses=(IDENTITY,), + gripper_values=(0.25,), + step_dt_s=0.1, + ) + plan = task_motion_plan_from_dense_trace( + trace, + request_digest="request", + snapshot_digest="snapshot", + backend="test", + backend_version="1", + seed=0, + goal=(), + ) + + command = plan.segments[0] + assert isinstance(command, GripperCommandSegment) + assert command.command is GripperCommandMode.POSITION + assert command.value == 0.25 + + +def test_interaction_split_rejects_nonduplicated_pose(): + shifted = tuple( + tuple(value + (0.01 if row == 0 and column == 3 else 0.0) for column, value in enumerate(values)) + for row, values in enumerate(IDENTITY) + ) + + with pytest.raises(ValueError, match="duplicate the preceding pose"): + DensePlanTrace( + eef_name="hand", + frame="base", + poses=(IDENTITY, shifted), + gripper_values=(1.0, -1.0), + step_dt_s=0.1, + ) + + +def test_interaction_split_rejects_nonduplicated_joint_seed(): + with pytest.raises(ValueError, match="duplicate the preceding joint seed"): + DensePlanTrace( + eef_name="hand", + frame="base", + poses=(IDENTITY, IDENTITY), + gripper_values=(1.0, -1.0), + step_dt_s=0.1, + joint_names=("joint",), + joint_positions=((0.0,), (0.1,)), + ) + + +def test_attachment_event_rejects_initial_sample_without_preceding_target(): + with pytest.raises(ValueError, match="preceding collision-checked dense sample"): + DensePlanTrace( + eef_name="hand", + frame="base", + poses=(IDENTITY,), + gripper_values=(-1.0,), + step_dt_s=0.1, + attachment_events=(DenseAttachmentEvent(0, "attach", "cube"),), + ) diff --git a/isaac_autodata_tests/core/autonomous/test_output_transaction.py b/isaac_autodata_tests/core/autonomous/test_output_transaction.py new file mode 100644 index 0000000..9a72886 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_output_transaction.py @@ -0,0 +1,582 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import h5py +import hashlib +import json +import os +import stat +from pathlib import Path + +import pytest + +import isaac_autodata_core.autonomous.output_transaction as output_transaction_module +from isaac_autodata_core.autonomous.output_transaction import ( + DatasetCommitUncertainError, + OutputTransaction, + RequestDirectoryAnchor, +) + + +def _write_dataset(transaction: OutputTransaction, *, episodes: int = 1, failed: bool = False) -> bytes: + targets = transaction.recording_targets + suffix = "_failed" if failed else "" + path = Path(targets.dataset_export_dir_path) / f"{targets.dataset_filename}{suffix}.hdf5" + with h5py.File(path, "w") as dataset: + dataset.attrs["format_version"] = 1 + data = dataset.create_group("data") + for index in range(episodes): + episode = data.create_group(f"demo_{index}") + episode.attrs["num_samples"] = 1 + episode.attrs["success"] = not failed + initial_robot = episode.create_group("initial_state/articulation/robot") + initial_robot.create_dataset("joint_position", data=[[float(index)]]) + episode.create_group("obs").create_dataset("joint_pos", data=[[float(index)]]) + episode.create_group("states").create_dataset("joint_pos", data=[[float(index)]]) + episode.create_dataset("actions", data=[[float(index)]]) + episode.create_dataset("processed_actions", data=[[float(index)]]) + return path.read_bytes() + + +def _write_zero_sample_failed_dataset(transaction: OutputTransaction) -> bytes: + targets = transaction.recording_targets + path = Path(targets.dataset_export_dir_path) / f"{targets.dataset_filename}_failed.hdf5" + with h5py.File(path, "w") as dataset: + dataset.attrs["format_version"] = 1 + episode = dataset.create_group("data/demo_0") + episode.attrs["num_samples"] = 0 + episode.attrs["success"] = False + episode.create_group("initial_state/articulation/robot").create_dataset( + "joint_position", + data=[[0.0]], + ) + return path.read_bytes() + + +def _reserve(tmp_path: Path, *, run_log: bool = True, keep_failed: bool = False) -> OutputTransaction: + return OutputTransaction.reserve( + request_directory=tmp_path, + dataset_path=tmp_path / "nested" / "episodes.hdf5", + run_log_path=tmp_path / "audit" / "run.jsonl" if run_log else None, + keep_failed=keep_failed, + ) + + +def test_transaction_publishes_validated_inode_and_terminal_commit(tmp_path: Path) -> None: + transaction = _reserve(tmp_path) + targets = transaction.recording_targets + staging_path = Path(targets.dataset_export_dir_path) + assert stat.S_IMODE(staging_path.stat().st_mode) == 0o700 + assert stat.S_IMODE((tmp_path / "nested").stat().st_mode) == 0o700 + assert stat.S_IMODE((tmp_path / "audit" / "run.jsonl").stat().st_mode) == 0o600 + original = _write_dataset(transaction, episodes=2) + assert not transaction.dataset_path.exists() + writer = transaction.open_run_log_writer() + assert writer is not None + writer.append({"record_type": "started"}) + + artifacts = transaction.publish( + run_log_writer=writer, + commit_record={"record_type": "run_committed"}, + expected_successful_episodes=2, + ) + writer.close() + transaction.close() + + assert transaction.dataset_path.read_bytes() == original + assert len(artifacts) == 1 + assert artifacts[0].sha256 == hashlib.sha256(original).hexdigest() + assert artifacts[0].episode_count == 2 + records = [json.loads(line) for line in (tmp_path / "audit" / "run.jsonl").read_text().splitlines()] + assert [record["record_type"] for record in records] == ["started", "run_committed"] + assert records[-1]["artifacts"] == [artifacts[0].to_dict()] + assert not staging_path.exists() + + +def test_publication_never_replaces_file_created_after_reservation(tmp_path: Path) -> None: + transaction = _reserve(tmp_path) + _write_dataset(transaction) + transaction.dataset_path.write_bytes(b"human-owned") + + with pytest.raises(FileExistsError): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert transaction.dataset_path.read_bytes() == b"human-owned" + + +def test_successful_link_is_rolled_back_if_postlink_identity_check_fails( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + original_stat = output_transaction_module.os.stat + fail_once = True + + def fail_final_stat(path: object, *args: object, **kwargs: object) -> os.stat_result: + nonlocal fail_once + if ( + fail_once + and path == transaction.dataset_path.name + and kwargs.get("dir_fd") == transaction._dataset_parent_fd + ): + fail_once = False + raise OSError("post-link identity probe failed") + return original_stat(path, *args, **kwargs) + + monkeypatch.setattr(output_transaction_module.os, "stat", fail_final_stat) + with pytest.raises(OSError, match="identity probe"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_publication_links_held_inode_if_staging_name_is_swapped( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + transaction = _reserve(tmp_path, run_log=False) + original = _write_dataset(transaction) + original_link = output_transaction_module._link_fd_to_name + + def swap_then_link(source_fd: int, destination_dir_fd: int, destination_name: str) -> None: + os.rename( + "episodes.hdf5", + "validated-original.hdf5", + src_dir_fd=transaction._staging_fd, + dst_dir_fd=transaction._staging_fd, + ) + malicious = os.open( + "episodes.hdf5", + os.O_WRONLY | os.O_CREAT | os.O_EXCL, + 0o600, + dir_fd=transaction._staging_fd, + ) + os.write(malicious, b"not the validated inode") + os.close(malicious) + original_link(source_fd, destination_dir_fd, destination_name) + + monkeypatch.setattr(output_transaction_module, "_link_fd_to_name", swap_then_link) + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert transaction.dataset_path.read_bytes() == original + + +def test_staged_symlink_is_rejected_without_reading_target(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + outside = tmp_path / "outside.hdf5" + outside.write_bytes(b"outside") + os.symlink(outside, "episodes.hdf5", dir_fd=transaction._staging_fd) + + with pytest.raises(OSError): + transaction.publish(run_log_writer=None, commit_record=None) + transaction.close() + + assert outside.read_bytes() == b"outside" + assert not transaction.dataset_path.exists() + + +def test_symlinked_output_parent_is_rejected(tmp_path: Path) -> None: + real = tmp_path / "real" + real.mkdir() + (tmp_path / "linked").symlink_to(real, target_is_directory=True) + + with pytest.raises(OSError): + OutputTransaction.reserve( + request_directory=tmp_path, + dataset_path=tmp_path / "linked" / "episodes.hdf5", + run_log_path=None, + keep_failed=False, + ) + + assert list(real.iterdir()) == [] + + +def test_dataset_parent_rename_after_reservation_prevents_misreported_publication(tmp_path: Path) -> None: + transaction = _reserve(tmp_path) + _write_dataset(transaction) + original_parent = tmp_path / "nested-original" + transaction.dataset_path.parent.rename(original_parent) + transaction.dataset_path.parent.mkdir(mode=0o700) + writer = transaction.open_run_log_writer() + + with pytest.raises(RuntimeError, match="dataset parent"): + transaction.publish( + run_log_writer=writer, + commit_record={"record_type": "run_committed"}, + expected_successful_episodes=1, + ) + assert writer is not None + writer.close() + transaction.close() + + assert not transaction.dataset_path.exists() + assert list(transaction.dataset_path.parent.iterdir()) == [] + + +def test_run_log_conflict_removes_private_staging(tmp_path: Path) -> None: + run_log = tmp_path / "audit" / "run.jsonl" + run_log.parent.mkdir() + run_log.write_text("owned\n", encoding="utf-8") + + with pytest.raises(FileExistsError): + _reserve(tmp_path) + + assert run_log.read_text(encoding="utf-8") == "owned\n" + dataset_parent = tmp_path / "nested" + assert list(dataset_parent.iterdir()) == [] + + +def test_reservation_failure_does_not_unlink_replacement_run_log( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + original_open = output_transaction_module._open_exclusive_run_log + + def replace_after_open(name: str, parent_fd: int) -> int: + descriptor = original_open(name, parent_fd) + os.unlink(name, dir_fd=parent_fd) + replacement = os.open( + name, + os.O_WRONLY | os.O_CREAT | os.O_EXCL | os.O_APPEND | os.O_NOFOLLOW, + 0o600, + dir_fd=parent_fd, + ) + os.write(replacement, b"replacement\n") + os.close(replacement) + return descriptor + + monkeypatch.setattr(output_transaction_module, "_open_exclusive_run_log", replace_after_open) + with pytest.raises(RuntimeError, match="held inode"): + _reserve(tmp_path) + + assert (tmp_path / "audit" / "run.jsonl").read_text() == "replacement\n" + + +def test_episode_count_mismatch_prevents_publication_and_commit(tmp_path: Path) -> None: + transaction = _reserve(tmp_path) + _write_dataset(transaction, episodes=1) + writer = transaction.open_run_log_writer() + assert writer is not None + writer.append({"record_type": "started"}) + + with pytest.raises(ValueError, match="expected 2"): + transaction.publish( + run_log_writer=writer, + commit_record={"record_type": "run_committed"}, + expected_successful_episodes=2, + ) + writer.close() + transaction.close() + + assert not transaction.dataset_path.exists() + records = [json.loads(line) for line in (tmp_path / "audit" / "run.jsonl").read_text().splitlines()] + assert [record["record_type"] for record in records] == ["started"] + + +def test_structurally_incomplete_hdf5_is_not_attested(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + targets = transaction.recording_targets + staged = Path(targets.dataset_export_dir_path) / f"{targets.dataset_filename}.hdf5" + with h5py.File(staged, "w") as dataset: + dataset.attrs["format_version"] = 1 + dataset.create_group("data").create_group("demo_0") + + with pytest.raises(ValueError, match="initial_state"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_numeric_success_metadata_is_not_accepted_as_boolean(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + targets = transaction.recording_targets + staged = Path(targets.dataset_export_dir_path) / f"{targets.dataset_filename}.hdf5" + with h5py.File(staged, "r+") as dataset: + dataset["data/demo_0"].attrs["success"] = 1 + + with pytest.raises(ValueError, match="success metadata"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_successful_episode_requires_initial_state_datasets(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + staged = Path(transaction.recording_targets.dataset_export_dir_path) / "episodes.hdf5" + with h5py.File(staged, "r+") as dataset: + del dataset["data/demo_0/initial_state/articulation"] + + with pytest.raises(ValueError, match="initial_state.*no datasets"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_successful_episode_rejects_empty_initial_state_dataset(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + staged = Path(transaction.recording_targets.dataset_export_dir_path) / "episodes.hdf5" + with h5py.File(staged, "r+") as dataset: + robot = dataset["data/demo_0/initial_state/articulation/robot"] + del robot["joint_position"] + robot.create_dataset("joint_position", shape=(0, 1), dtype="f4") + + with pytest.raises(ValueError, match="initial_state.*must not be empty"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +@pytest.mark.parametrize("value", [float("nan"), float("inf")], ids=("nan", "inf")) +@pytest.mark.parametrize( + "dataset_path", + [ + "processed_actions", + "obs/joint_pos", + "states/joint_pos", + "initial_state/articulation/robot/joint_position", + ], +) +def test_nonfinite_episode_data_is_not_published(tmp_path: Path, dataset_path: str, value: float) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + staged = Path(transaction.recording_targets.dataset_export_dir_path) / "episodes.hdf5" + with h5py.File(staged, "r+") as dataset: + dataset[f"data/demo_0/{dataset_path}"][0, 0] = value + + with pytest.raises(ValueError, match="contains non-finite values"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +@pytest.mark.parametrize( + "dataset_path", + [ + "processed_actions", + "obs/joint_pos", + "states/joint_pos", + "initial_state/articulation/robot/joint_position", + ], +) +def test_nonnumeric_episode_data_is_not_published(tmp_path: Path, dataset_path: str) -> None: + transaction = _reserve(tmp_path, run_log=False) + _write_dataset(transaction) + staged = Path(transaction.recording_targets.dataset_export_dir_path) / "episodes.hdf5" + with h5py.File(staged, "r+") as dataset: + episode = dataset["data/demo_0"] + del episode[dataset_path] + episode.create_dataset(dataset_path, data=[[b"not numeric"]]) + + with pytest.raises(ValueError, match="must be numeric"): + transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_nested_product10_like_episode_is_published(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, run_log=False) + targets = transaction.recording_targets + staged = Path(targets.dataset_export_dir_path) / f"{targets.dataset_filename}.hdf5" + with h5py.File(staged, "w") as dataset: + dataset.attrs["format_version"] = 1 + episode = dataset.create_group("data/demo_0") + episode.attrs["num_samples"] = 2 + episode.attrs["success"] = True + initial_state = episode.create_group("initial_state") + initial_state.create_dataset("articulation/robot/joint_position", data=[[0.0, 0.1]]) + initial_state.create_dataset("rigid_object/pick_cube/root_pose", data=[[0.0] * 7]) + obs = episode.create_group("obs") + obs.create_dataset("policy/joint_pos", data=[[0.0, 0.1], [0.1, 0.2]]) + states = episode.create_group("states") + states.create_dataset("articulation/robot/joint_position", data=[[0.0, 0.1], [0.1, 0.2]]) + states.create_dataset("rigid_object/pick_cube/root_pose", data=[[0.0] * 7, [0.1] * 7]) + episode.create_dataset("actions", data=[[0.0], [0.1]]) + episode.create_dataset("processed_actions", data=[[0.0, 1.0], [0.1, 1.0]]) + + artifacts = transaction.publish( + run_log_writer=None, + commit_record=None, + expected_successful_episodes=1, + ) + transaction.close() + + assert len(artifacts) == 1 + assert artifacts[0].episode_count == 1 + assert transaction.dataset_path.exists() + + +def test_post_append_ledger_replacement_is_ambiguous_and_does_not_roll_back_dataset(tmp_path: Path) -> None: + transaction = _reserve(tmp_path) + _write_dataset(transaction) + writer = transaction.open_run_log_writer() + assert writer is not None + writer.append({"record_type": "started"}) + original_append = writer.append + + def replace_after_append(record: dict[str, object]) -> None: + original_append(record) + assert transaction._run_log_parent_fd is not None + assert transaction.run_log_path is not None + os.unlink(transaction.run_log_path.name, dir_fd=transaction._run_log_parent_fd) + replacement = os.open( + transaction.run_log_path.name, + os.O_WRONLY | os.O_CREAT | os.O_EXCL | os.O_APPEND | os.O_NOFOLLOW, + 0o600, + dir_fd=transaction._run_log_parent_fd, + ) + os.write(replacement, b'{"attacker":true}\n') + os.close(replacement) + + writer.append = replace_after_append + with pytest.raises(DatasetCommitUncertainError, match="do not retry"): + transaction.publish( + run_log_writer=writer, + commit_record={"record_type": "run_committed"}, + expected_successful_episodes=1, + ) + writer.close() + transaction.close() + + assert transaction.dataset_path.exists() + assert json.loads((tmp_path / "audit" / "run.jsonl").read_text()) == {"attacker": True} + + +def test_failed_dataset_is_validated_and_published_as_a_separate_artifact(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, keep_failed=True) + successful = _write_dataset(transaction, episodes=2) + failed = _write_dataset(transaction, episodes=3, failed=True) + + artifacts = transaction.publish( + run_log_writer=None, + commit_record=None, + require_failed_dataset=True, + expected_successful_episodes=2, + expected_failed_episodes=3, + ) + transaction.close() + + assert [artifact.kind for artifact in artifacts] == ["successful", "failed"] + assert transaction.dataset_path.read_bytes() == successful + assert transaction.failed_dataset_path is not None + assert transaction.failed_dataset_path.read_bytes() == failed + + +def test_zero_sample_failed_episode_without_task_stream_is_published(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, keep_failed=True) + _write_dataset(transaction) + failed = _write_zero_sample_failed_dataset(transaction) + + artifacts = transaction.publish( + run_log_writer=None, + commit_record=None, + require_failed_dataset=True, + expected_successful_episodes=1, + expected_failed_episodes=1, + ) + transaction.close() + + assert [artifact.kind for artifact in artifacts] == ["successful", "failed"] + assert transaction.failed_dataset_path is not None + assert transaction.failed_dataset_path.read_bytes() == failed + + +def test_nonzero_failed_episode_still_requires_complete_task_stream(tmp_path: Path) -> None: + transaction = _reserve(tmp_path, keep_failed=True) + _write_dataset(transaction) + _write_zero_sample_failed_dataset(transaction) + staged = Path(transaction.recording_targets.dataset_export_dir_path) / "episodes_failed.hdf5" + with h5py.File(staged, "r+") as dataset: + dataset["data/demo_0"].attrs["num_samples"] = 1 + + with pytest.raises(ValueError, match="has no 'obs' group"): + transaction.publish( + run_log_writer=None, + commit_record=None, + require_failed_dataset=True, + expected_successful_episodes=1, + expected_failed_episodes=1, + ) + transaction.close() + + assert not transaction.dataset_path.exists() + + +def test_request_anchor_detects_parent_directory_replacement(tmp_path: Path) -> None: + request_directory = tmp_path / "request-root" + request_directory.mkdir() + request_path = request_directory / "request.yaml" + request_path.write_text("schema_version: 1\n", encoding="utf-8") + anchor = RequestDirectoryAnchor.open(request_path) + original_directory = tmp_path / "original-root" + request_directory.rename(original_directory) + request_directory.mkdir() + (request_directory / "request.yaml").write_text("schema_version: 1\n", encoding="utf-8") + + with pytest.raises(RuntimeError, match="renamed or replaced"): + anchor.verify_current() + anchor.close() + + +def test_request_anchor_detects_request_file_replacement(tmp_path: Path) -> None: + request_path = tmp_path / "request.yaml" + request_path.write_text("schema_version: 1\n", encoding="utf-8") + anchor = RequestDirectoryAnchor.open(request_path) + request_path.rename(tmp_path / "old-request.yaml") + request_path.write_text("schema_version: 1\n", encoding="utf-8") + + with pytest.raises(RuntimeError, match="identity changed"): + anchor.verify_current() + anchor.close() diff --git a/isaac_autodata_tests/core/autonomous/test_run_log.py b/isaac_autodata_tests/core/autonomous/test_run_log.py new file mode 100644 index 0000000..3b2723d --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_run_log.py @@ -0,0 +1,175 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import json +import os +from collections.abc import Mapping + +import pytest + +import isaac_autodata_core.autonomous.run_log as run_log_module +from isaac_autodata_core.autonomous.run_log import ( + MAX_RUN_RECORD_COLLECTION_ITEMS, + MAX_RUN_RECORD_JSON_NODES, + RunLogWriter, + RunLogWriteUncertainError, + normalize_run_record, + safe_exception_record, +) + + +class _MisreportedMapping(Mapping): + def __getitem__(self, key): + return key + + def __iter__(self): + yield "first" + yield "unexpected" + + def __len__(self): + return 1 + + +def test_run_log_writer_appends_complete_json_lines(tmp_path): + path = tmp_path / "attempts.jsonl" + writer = RunLogWriter(path) + + writer.append({"attempt_id": "attempt-1", "status": "failed"}) + writer.append({"attempt_id": "attempt-2", "status": "succeeded"}) + writer.close() + + lines = path.read_text(encoding="utf-8").splitlines() + assert [json.loads(line)["attempt_id"] for line in lines] == ["attempt-1", "attempt-2"] + + +def test_run_log_writer_requires_jsonl_extension(tmp_path): + with pytest.raises(ValueError, match="jsonl"): + RunLogWriter(tmp_path / "attempts.json") + + +def test_run_log_writer_rejects_oversized_record(tmp_path): + writer = RunLogWriter(tmp_path / "attempts.jsonl", max_record_bytes=1024) + with pytest.raises(ValueError, match="maximum"): + writer.append({"payload": "x" * 2000}) + writer.close() + + +def test_run_log_writer_rejects_non_json_objects(tmp_path): + writer = RunLogWriter(tmp_path / "attempts.jsonl") + with pytest.raises(ValueError, match="unsupported"): + writer.append({"payload": object()}) + writer.close() + + +def test_run_log_value_normalization_matches_writer_types_and_byte_bound(): + normalized = normalize_run_record( + {"nested": {"values": (1, 2, 3)}}, + max_serialized_bytes=128, + ) + + assert normalized == {"nested": {"values": [1, 2, 3]}} + with pytest.raises(ValueError, match="maximum is 32"): + normalize_run_record({"payload": "x" * 64}, max_serialized_bytes=32) + + +def test_run_log_value_normalization_rejects_excessive_depth_and_collection_items(): + nested = {} + for _ in range(14): + nested = {"nested": nested} + + with pytest.raises(ValueError, match="nesting depth"): + normalize_run_record(nested) + with pytest.raises(ValueError, match="maximum item count"): + normalize_run_record([None] * (MAX_RUN_RECORD_COLLECTION_ITEMS + 1)) + + shared_values = [None] * 1_000 + repeated_mapping = { + str(index): shared_values for index in range(MAX_RUN_RECORD_JSON_NODES // len(shared_values) + 1) + } + with pytest.raises(ValueError, match="maximum JSON node count"): + normalize_run_record(repeated_mapping) + with pytest.raises(ValueError, match="misreports"): + normalize_run_record(_MisreportedMapping()) + + +def test_partial_append_permanently_poisons_ledger_without_later_writes(tmp_path, monkeypatch): + path = tmp_path / "attempts.jsonl" + writer = RunLogWriter(path) + original_write = run_log_module.os.write + first_write = True + + def partial_then_fail(descriptor, data): + nonlocal first_write + if first_write: + first_write = False + original_write(descriptor, data[:7]) + raise OSError("simulated partial append") + return original_write(descriptor, data) + + monkeypatch.setattr(run_log_module.os, "write", partial_then_fail) + with pytest.raises(RunLogWriteUncertainError, match="durability is unknown"): + writer.append({"record_type": "first"}) + poisoned_size = path.stat().st_size + + with pytest.raises(RunLogWriteUncertainError, match="poisoned"): + writer.append({"record_type": "must_not_be_written"}) + writer.close() + + assert poisoned_size == 7 + assert path.stat().st_size == poisoned_size + + +def test_run_log_writer_uses_and_closes_held_append_descriptor(tmp_path): + path = tmp_path / "attempts.jsonl" + descriptor = os.open( + path, + os.O_WRONLY | os.O_CREAT | os.O_EXCL | os.O_APPEND | os.O_NOFOLLOW, + 0o600, + ) + writer = RunLogWriter(path, fd=descriptor) + + writer.append({"record_type": "held_fd"}) + writer.close() + + assert json.loads(path.read_text())["record_type"] == "held_fd" + with pytest.raises(OSError): + os.fstat(descriptor) + + +def test_run_log_writer_rejects_descriptor_without_append(tmp_path): + path = tmp_path / "attempts.jsonl" + descriptor = os.open(path, os.O_WRONLY | os.O_CREAT | os.O_EXCL, 0o600) + try: + with pytest.raises(ValueError, match="O_APPEND"): + RunLogWriter(path, fd=descriptor) + finally: + os.close(descriptor) + + +def test_run_log_writer_does_not_follow_final_symlink(tmp_path): + target = tmp_path / "target.jsonl" + target.write_text("owned\n", encoding="utf-8") + (tmp_path / "attempts.jsonl").symlink_to(target) + + with pytest.raises(OSError): + RunLogWriter(tmp_path / "attempts.jsonl") + + assert target.read_text(encoding="utf-8") == "owned\n" + + +def test_safe_exception_record_is_bounded_and_optional_traceback(): + try: + raise RuntimeError("bad\x00message" + "x" * 3000) + except RuntimeError as exc: + short = safe_exception_record(exc) + debug = safe_exception_record(exc, include_traceback=True) + + assert short["exception_type"].endswith("RuntimeError") + assert "\x00" not in short["message"] + assert len(short["message"]) == 2048 + assert "traceback" not in short + assert "RuntimeError" in debug["traceback"] diff --git a/isaac_autodata_tests/core/autonomous/test_task_motion.py b/isaac_autodata_tests/core/autonomous/test_task_motion.py new file mode 100644 index 0000000..f4d42a4 --- /dev/null +++ b/isaac_autodata_tests/core/autonomous/test_task_motion.py @@ -0,0 +1,247 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import json +import math + +import pytest + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + ConcurrentGroupSegment, + ExecutionEvent, + ExecutionEventType, + ExecutionOutcome, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + RobotStateSnapshot, + SceneObjectSnapshot, + SceneSnapshot, + TaskMotionPlan, + WaitSegment, + make_stable_id, + matrix4, + matrix4_error, + matrix4_inverse, + matrix4_multiply, +) + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +def _scene_snapshot() -> SceneSnapshot: + return SceneSnapshot( + snapshot_id="snapshot-1", + captured_at_s=4.5, + env_id=0, + robot=RobotStateSnapshot( + robot_id="franka", + joint_names=("joint_1", "joint_2"), + joint_positions=(0.1, -0.2), + eef_poses={"franka": IDENTITY}, + held_objects={"franka": None}, + ), + objects=( + SceneObjectSnapshot( + semantic_id="cube", + scene_id="cube_1", + pose=IDENTITY, + roles=("support", "graspable", "support"), + ), + ), + metadata={"seed": 7}, + ) + + +def _plan() -> TaskMotionPlan: + move = CartesianTrajectorySegment( + segment_id="move-1", + eef_name="franka", + frame="robot_base", + poses=(IDENTITY,), + joint_seed_names=("joint_1", "joint_2"), + joint_seeds=((0.1, -0.2),), + expected_postconditions=(GoalPredicate("near", "franka", "cube_1"),), + ) + close = GripperCommandSegment( + segment_id="close-1", + depends_on=(move.segment_id,), + eef_name="franka", + command=GripperCommandMode.CLOSE, + settle_steps=4, + ) + attach = AttachIntentSegment( + segment_id="attach-1", + depends_on=(close.segment_id,), + eef_name="franka", + object_name="cube_1", + verifier="contact_and_relative_motion_v1", + ) + return TaskMotionPlan( + plan_id="plan-1", + request_digest="request-digest", + snapshot_digest="snapshot-digest", + backend="schedulestream_custream", + backend_version="test", + seed=7, + segments=(move, close, attach), + goal=(GoalPredicate("on", "cube_1", "table"),), + metadata={"motion_backend": "curobo_v1"}, + ) + + +def test_stable_id_is_deterministic_and_namespaced(): + assert make_stable_id("segment", "a", 1) == make_stable_id("segment", "a", 1) + assert make_stable_id("segment", "a", 1).startswith("segment-") + assert make_stable_id("segment", "a", 1) != make_stable_id("segment", "a", 2) + + +@pytest.mark.parametrize( + "pose,match", + [ + (((1.0, 0.0),), "shape"), + ( + ( + (2.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + "unit norm", + ), + ( + ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, -1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + "determinant", + ), + ], +) +def test_matrix4_rejects_non_rigid_transforms(pose, match): + with pytest.raises(ValueError, match=match): + matrix4(pose) + + +def test_matrix4_rigid_operations_are_consistent() -> None: + transform = ( + (0.0, -1.0, 0.0, 0.1), + (1.0, 0.0, 0.0, -0.2), + (0.0, 0.0, 1.0, 0.3), + (0.0, 0.0, 0.0, 1.0), + ) + + product = matrix4_multiply(transform, matrix4_inverse(transform)) + assert tuple(value for row in product for value in row) == pytest.approx( + tuple(value for row in IDENTITY for value in row) + ) + position_error, rotation_error = matrix4_error(IDENTITY, transform) + assert position_error == pytest.approx((0.1**2 + 0.2**2 + 0.3**2) ** 0.5) + assert rotation_error == pytest.approx(math.pi / 2) + + +def test_scene_snapshot_round_trip_and_digest_are_deterministic(): + snapshot = _scene_snapshot() + rebuilt = SceneSnapshot.from_dict(snapshot.to_dict()) + + assert rebuilt == snapshot + assert rebuilt.digest == snapshot.digest + assert rebuilt.objects[0].roles == ("graspable", "support") + + +def test_task_motion_plan_round_trip_preserves_symbolic_segments(): + plan = _plan() + rebuilt = TaskMotionPlan.from_dict(json.loads(plan.canonical_json())) + + assert rebuilt == plan + assert rebuilt.digest == plan.digest + assert rebuilt.segments[2].kind.value == "attach_intent" + assert isinstance(rebuilt.segments[2], AttachIntentSegment) + + +def test_plan_rejects_unknown_dependencies(): + segment = WaitSegment(segment_id="wait", depends_on=("missing",), steps=1) + with pytest.raises(ValueError, match="unknown dependencies"): + TaskMotionPlan( + plan_id="plan", + request_digest="request", + snapshot_digest="snapshot", + backend="test", + backend_version="1", + seed=0, + segments=(segment,), + goal=(), + ) + + +def test_plan_rejects_dependency_cycles(): + first = WaitSegment(segment_id="first", depends_on=("second",), steps=1) + second = WaitSegment(segment_id="second", depends_on=("first",), steps=1) + with pytest.raises(ValueError, match="dependency cycle"): + TaskMotionPlan( + plan_id="plan", + request_digest="request", + snapshot_digest="snapshot", + backend="test", + backend_version="1", + seed=0, + segments=(first, second), + goal=(), + ) + + +def test_plan_rejects_unknown_concurrent_members(): + group = ConcurrentGroupSegment(segment_id="group", member_segment_ids=("missing",)) + with pytest.raises(ValueError, match="unknown members"): + TaskMotionPlan( + plan_id="plan", + request_digest="request", + snapshot_digest="snapshot", + backend="test", + backend_version="1", + seed=0, + segments=(group,), + goal=(), + ) + + +def test_execution_event_is_bounded_json_contract(): + event = ExecutionEvent( + event_id="event-1", + attempt_id="attempt-1", + plan_id="plan-1", + segment_id="attach-1", + event_type=ExecutionEventType.GRASP_INTENT, + outcome=ExecutionOutcome.PENDING, + monotonic_time_s=3.2, + verifier="contact_and_relative_motion_v1", + metadata={"candidate_id": "grasp-2"}, + ) + + assert event.to_dict()["event_type"] == "grasp_intent" + assert event.to_dict()["outcome"] == "pending" + + +def test_metadata_rejects_non_finite_numbers(): + with pytest.raises(ValueError, match="finite"): + _scene_snapshot().__class__( + snapshot_id="snapshot", + captured_at_s=0.0, + env_id=0, + robot=_scene_snapshot().robot, + objects=(), + metadata={"bad": float("nan")}, + ) diff --git a/isaac_autodata_tests/docker/__init__.py b/isaac_autodata_tests/docker/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/docker/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/docker/test_run_autonomous_task.py b/isaac_autodata_tests/docker/test_run_autonomous_task.py new file mode 100644 index 0000000..def6eff --- /dev/null +++ b/isaac_autodata_tests/docker/test_run_autonomous_task.py @@ -0,0 +1,375 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import os +import shutil +import socket +import subprocess +from pathlib import Path + +REPO_ROOT = Path(__file__).resolve().parents[2] +LAUNCHER = REPO_ROOT / "docker" / "run_autonomous_task.sh" +CONTAINER_ENTRYPOINT = REPO_ROOT / "docker" / "autonomous_entrypoint.sh" +IMAGE_ID = f"sha256:{'a' * 64}" +SCHEDULESTREAM_COMMIT = "b" * 40 +BASE_IMAGE_ID = f"sha256:{'c' * 64}" +V1_IMAGE_CONTRACT = f"custream|v1|{SCHEDULESTREAM_COMMIT}|{BASE_IMAGE_ID}" + + +def _write_fake_docker(root: Path) -> tuple[Path, Path, Path]: + bin_dir = root / "bin" + bin_dir.mkdir() + run_args_path = root / "docker-run-args.bin" + inspect_args_path = root / "docker-inspect-args.bin" + docker_path = bin_dir / "docker" + docker_path.write_text( + "#!/bin/bash\n" + "set -euo pipefail\n" + ': "${FAKE_DOCKER_RUN_ARGS:?}" "${FAKE_DOCKER_INSPECT_ARGS:?}"\n' + 'if [[ "$1" == "image" && "$2" == "inspect" ]]; then\n' + ' printf \'%s\\0\' "$@" >>"${FAKE_DOCKER_INSPECT_ARGS}"\n' + " printf '\\0' >>\"${FAKE_DOCKER_INSPECT_ARGS}\"\n" + ' if [[ "$4" == "{{.Id}}" ]]; then\n' + " printf '%s\\n' \"${FAKE_DOCKER_IMAGE_ID:?}\"\n" + " else\n" + " printf '%s\\n' \"${FAKE_DOCKER_IMAGE_CONTRACT:?}\"\n" + " fi\n" + " exit 0\n" + "fi\n" + 'if [[ "$1" == "run" ]]; then\n' + ' printf \'%s\\0\' "$@" >"${FAKE_DOCKER_RUN_ARGS}"\n' + ' exit "${FAKE_DOCKER_EXIT:-0}"\n' + "fi\n" + "printf 'unexpected docker invocation: %s\\n' \"$*\" >&2\n" + "exit 97\n", + encoding="utf-8", + ) + docker_path.chmod(0o755) + return bin_dir, run_args_path, inspect_args_path + + +def _write_fake_xauth(bin_dir: Path) -> None: + xauth_path = bin_dir / "xauth" + xauth_path.write_text( + "#!/bin/bash\n" + "set -euo pipefail\n" + 'case "$3" in\n' + " nlist) printf '0000ffff0123456789abcdef\\n' ;;\n" + ' nmerge) cat >"$2" ;;\n' + " *) exit 2 ;;\n" + "esac\n", + encoding="utf-8", + ) + xauth_path.chmod(0o755) + + +def _task_yaml(root: Path) -> Path: + task_path = root / "task.yaml" + task_path.write_text("schema_version: 1\n", encoding="utf-8") + return task_path + + +def _run_launcher( + root: Path, + *arguments: str, + launcher: Path = LAUNCHER, + accept_eula: str | None = "Y", + docker_exit: int = 0, + image_contract: str = V1_IMAGE_CONTRACT, + fake_xauth: bool = False, + extra_env: dict[str, str] | None = None, +) -> tuple[subprocess.CompletedProcess[str], list[str], list[list[str]]]: + bin_dir, run_args_path, inspect_args_path = _write_fake_docker(root) + if fake_xauth: + _write_fake_xauth(bin_dir) + env = os.environ.copy() + env["PATH"] = f"{bin_dir}:{env['PATH']}" + env["FAKE_DOCKER_RUN_ARGS"] = str(run_args_path) + env["FAKE_DOCKER_INSPECT_ARGS"] = str(inspect_args_path) + env["FAKE_DOCKER_EXIT"] = str(docker_exit) + env["FAKE_DOCKER_IMAGE_ID"] = IMAGE_ID + env["FAKE_DOCKER_IMAGE_CONTRACT"] = image_contract + if accept_eula is None: + env.pop("ACCEPT_EULA", None) + else: + env["ACCEPT_EULA"] = accept_eula + if extra_env is not None: + env.update(extra_env) + + result = subprocess.run( + [str(launcher), *arguments], + cwd=root, + env=env, + check=False, + capture_output=True, + text=True, + timeout=30, + ) + docker_run_args = [] + if run_args_path.exists(): + encoded_args = run_args_path.read_bytes().split(b"\0") + docker_run_args = [value.decode() for value in encoded_args if value] + + docker_inspect_args = [] + if inspect_args_path.exists(): + for encoded_call in inspect_args_path.read_bytes().split(b"\0\0"): + if encoded_call: + docker_inspect_args.append([value.decode() for value in encoded_call.split(b"\0") if value]) + return result, docker_run_args, docker_inspect_args + + +def _option_values(arguments: list[str], option: str) -> list[str]: + return [arguments[index + 1] for index, value in enumerate(arguments[:-1]) if value == option] + + +def test_headless_launch_has_scoped_mounts_and_v1_caches(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + + result, docker_args, inspect_calls = _run_launcher(tmp_path, "--run-dir", str(run_dir), str(task_path)) + + assert result.returncode == 0, result.stderr + assert result.stdout == f"Run directory: {run_dir.resolve()}\n" + assert [call[-1] for call in inspect_calls] == ["isaac_autodata:schedulestream-v1", IMAGE_ID] + assert docker_args[0] == "run" + assert _option_values(docker_args, "--security-opt") == ["no-new-privileges:true"] + assert _option_values(docker_args, "--user") == ["0:0"] + assert _option_values(docker_args, "--entrypoint") == ["/workspaces/isaac_autodata/docker/autonomous_entrypoint.sh"] + + env_values = _option_values(docker_args, "--env") + assert "ACCEPT_EULA" in env_values + assert "ACCEPT_EULA=Y" not in env_values + assert f"DOCKER_RUN_USER_ID={os.getuid()}" in env_values + assert f"DOCKER_RUN_GROUP_ID={os.getgid()}" in env_values + + mounts = _option_values(docker_args, "--mount") + assert f"type=bind,src={REPO_ROOT},dst=/workspaces/isaac_autodata,readonly" in mounts + assert f"type=bind,src={run_dir.resolve()},dst=/autonomous-run" in mounts + task_copy = run_dir / "task.yaml" + assert f"type=bind,src={task_copy.resolve()},dst=/autonomous-run/task.yaml,readonly" in mounts + assert task_copy.read_bytes() == task_path.read_bytes() + assert task_copy.stat().st_uid == os.getuid() + assert task_copy.stat().st_mode & 0o777 == 0o400 + + cache_namespace = f"isaac-autodata-isaac-sim-5-1-v1-{IMAGE_ID[7:19]}-u{os.getuid()}" + assert f"type=volume,src={cache_namespace}-kit,dst=/isaac-sim/kit/cache" in mounts + assert f"type=volume,src={cache_namespace}-ov,dst=/autodata-home/.cache/ov" in mounts + assert f"type=volume,src={cache_namespace}-warp,dst=/autodata-home/.cache/warp" in mounts + assert f"type=volume,src={cache_namespace}-gl,dst=/autodata-home/.cache/nvidia/GLCache" in mounts + assert f"type=volume,src={cache_namespace}-compute,dst=/autodata-home/.nv/ComputeCache" in mounts + assert all("umi" not in argument.lower() for argument in docker_args) + + assert docker_args[-4:] == [ + IMAGE_ID, + "/isaac-sim/python.sh", + "/workspaces/isaac_autodata/isaac_autodata_examples/generate_task_dataset.py", + "/autonomous-run/task.yaml", + ] + assert "--gui" not in docker_args + assert not any("/tmp/.X11-unix" in mount for mount in mounts) + + +def test_gui_maps_x11_and_forwards_gui_flag(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + host_xauthority = tmp_path / "host.Xauthority" + host_xauthority.write_text("host cookie database\n", encoding="utf-8") + runtime_dir = tmp_path / "runtime" + runtime_dir.mkdir() + + x11_socket = socket.socket(socket.AF_UNIX) + x11_socket_path = None + try: + for display_number in range(200, 1000): + candidate = Path(f"/tmp/.X11-unix/X{display_number}") + try: + x11_socket.bind(str(candidate)) + except OSError: + continue + x11_socket_path = candidate + break + assert x11_socket_path is not None + display = f":{x11_socket_path.name.removeprefix('X')}" + + result, docker_args, _ = _run_launcher( + tmp_path, + "--gui", + "--run-dir", + str(run_dir), + str(task_path), + fake_xauth=True, + extra_env={ + "DISPLAY": display, + "XAUTHORITY": str(host_xauthority), + "XDG_RUNTIME_DIR": str(runtime_dir), + }, + ) + finally: + x11_socket.close() + if x11_socket_path is not None: + x11_socket_path.unlink(missing_ok=True) + + assert result.returncode == 0, result.stderr + env_values = _option_values(docker_args, "--env") + assert f"DISPLAY={display}" in env_values + assert "XAUTHORITY=/autodata-xauthority" in env_values + assert "QT_X11_NO_MITSHM=1" in env_values + mounts = _option_values(docker_args, "--mount") + assert "type=bind,src=/tmp/.X11-unix,dst=/tmp/.X11-unix,readonly" in mounts + xauthority_mount = next(mount for mount in mounts if mount.endswith("dst=/autodata-xauthority,readonly")) + minimized_xauthority = Path(xauthority_mount.removeprefix("type=bind,src=").split(",dst=", maxsplit=1)[0]) + assert not minimized_xauthority.exists() + assert list(runtime_dir.iterdir()) == [] + assert docker_args[-1] == "--gui" + + +def test_eula_must_be_explicit_before_run_directory_creation(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + + result, docker_args, inspect_calls = _run_launcher( + tmp_path, + "--run-dir", + str(run_dir), + str(task_path), + accept_eula=None, + ) + + assert result.returncode == 2 + assert "ACCEPT_EULA=Y" in result.stderr + assert docker_args == [] + assert inspect_calls == [] + assert not run_dir.exists() + + +def test_docker_exit_code_is_returned_after_printing_run_directory(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + + result, docker_args, _ = _run_launcher( + tmp_path, + "--run-dir", + str(run_dir), + str(task_path), + docker_exit=37, + ) + + assert result.returncode == 37 + assert result.stdout == f"Run directory: {run_dir.resolve()}\n" + assert docker_args[0] == "run" + + +def test_invalid_image_labels_are_rejected_before_docker_run(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + + invalid_contract = f"custream2|v1|{SCHEDULESTREAM_COMMIT}|{BASE_IMAGE_ID}" + result, docker_args, inspect_calls = _run_launcher( + tmp_path, + "--run-dir", + str(run_dir), + str(task_path), + image_contract=invalid_contract, + ) + + assert result.returncode == 2 + assert "image is not a supported ScheduleStream/cuRobo runtime" in result.stderr + assert docker_args == [] + assert len(inspect_calls) == 2 + assert not run_dir.exists() + + +def test_default_run_directory_is_unique_and_below_dataset_root(tmp_path: Path) -> None: + fake_repo = tmp_path / "repo" + docker_dir = fake_repo / "docker" + docker_dir.mkdir(parents=True) + launcher = docker_dir / LAUNCHER.name + entrypoint = docker_dir / CONTAINER_ENTRYPOINT.name + shutil.copy2(LAUNCHER, launcher) + shutil.copy2(CONTAINER_ENTRYPOINT, entrypoint) + task_path = _task_yaml(tmp_path) + + result, docker_args, _ = _run_launcher(tmp_path, str(task_path), launcher=launcher) + + assert result.returncode == 0, result.stderr + run_dir = Path(result.stdout.removeprefix("Run directory: ").strip()) + assert run_dir.parent == fake_repo / "datasets" / "autonomous_runs" + assert run_dir.name.startswith("run.") + assert run_dir.is_dir() + assert f"type=bind,src={fake_repo},dst=/workspaces/isaac_autodata,readonly" in _option_values( + docker_args, "--mount" + ) + + +def test_repository_root_is_rejected_as_writable_run_directory(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + + result, docker_args, _ = _run_launcher(tmp_path, "--run-dir", str(REPO_ROOT), str(task_path)) + + assert result.returncode == 2 + assert "run directory is too broad" in result.stderr + assert docker_args == [] + + +def test_existing_different_task_copy_is_not_overwritten(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + run_dir.mkdir() + task_copy = run_dir / "task.yaml" + existing_contents = b"schema_version: existing\n" + task_copy.write_bytes(existing_contents) + + result, docker_args, _ = _run_launcher( + tmp_path, + "--run-dir", + str(run_dir), + str(task_path), + ) + + assert result.returncode == 2 + assert "run directory already contains task.yaml" in result.stderr + assert docker_args == [] + assert task_copy.read_bytes() == existing_contents + assert task_copy.stat().st_uid == os.getuid() + + +def test_explicit_headless_is_accepted(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + run_dir = tmp_path / "run" + + result, docker_args, _ = _run_launcher( + tmp_path, + "--headless", + "--run-dir", + str(run_dir), + str(task_path), + ) + + assert result.returncode == 0, result.stderr + assert "--gui" not in docker_args + assert not any("/tmp/.X11-unix" in argument for argument in docker_args) + + +def test_gui_and_headless_conflict_before_docker_inspection(tmp_path: Path) -> None: + task_path = _task_yaml(tmp_path) + + result, docker_args, inspect_calls = _run_launcher( + tmp_path, + "--gui", + "--headless", + str(task_path), + ) + + assert result.returncode == 2 + assert "conflicts" in result.stderr + assert docker_args == [] + assert inspect_calls == [] + + +def test_shell_scripts_have_valid_bash_syntax() -> None: + for script in (LAUNCHER, CONTAINER_ENTRYPOINT): + subprocess.run(["bash", "-n", str(script)], check=True, timeout=10) diff --git a/isaac_autodata_tests/examples/__init__.py b/isaac_autodata_tests/examples/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/examples/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/examples/autonomous/__init__.py b/isaac_autodata_tests/examples/autonomous/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/examples/autonomous/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/examples/autonomous/test_compile_task_request.py b/isaac_autodata_tests/examples/autonomous/test_compile_task_request.py new file mode 100644 index 0000000..28a674f --- /dev/null +++ b/isaac_autodata_tests/examples/autonomous/test_compile_task_request.py @@ -0,0 +1,502 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import io +import json +import stat +import subprocess +import sys +from collections.abc import Callable +from pathlib import Path +from typing import Any + +import pytest + +from isaac_autodata_examples.compile_task_request import ExitCode, run_cli +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.task_request_types import ( + ArenaCompilationResult, + CompiledTaskRequest, + GenerationConfig, + GoalStage, + MotionBackend, + PlannerBackend, + PlannerConfig, + ResolvedOutputConfig, + SpatialGoalConstraint, + canonical_json, + sha256_json, +) +from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + CuroboApiGeneration, + CuroboRuntimeCapabilities, + DistributionIdentity, + select_schedulestream_backend, +) + + +def _resolved_request(request_directory: Path) -> CompiledTaskRequest: + canonical_request = { + "environment": {"intent": {"task": "pick cube into bowl"}}, + "generation": { + "successful_episodes": 3, + "max_attempts": 9, + "num_envs": 2, + "seed": 17, + }, + "name": "franka-pick-cube-into-bowl", + "output": { + "dataset": "outputs/dataset.hdf5", + "keep_failed": False, + "run_log": "outputs/run_log.jsonl", + }, + "planner": { + "animate": False, + "backend": "schedulestream", + "batch_size": 16, + "collisions": True, + "interpolation_dt_s": 0.04, + "max_time_s": 10.0, + "motion_backend": "auto", + "profile": False, + }, + "schema_version": 1, + } + linked_graph = { + "env_name": "Agentic-Franka-Pick-Cube-Into-Bowl-v0", + "state_specs": [{ + "id": "success", + "spatial_constraints": [{ + "id": "cube-in-bowl", + "kind": "inside", + "params": {}, + "reference": "bowl", + "subject": "cube", + }], + }], + "tasks": [{"id": "pick-place", "kind": "pick_place", "success_state_spec_id": "success"}], + } + constraint = SpatialGoalConstraint( + id="cube-in-bowl", + kind="inside", + subject="cube", + reference="bowl", + params_json=canonical_json({}), + ) + arena = ArenaCompilationResult( + initial_graph_json=canonical_json(linked_graph), + linked_graph_json=canonical_json(linked_graph), + compiler_trace=(), + graph_digest=sha256_json(linked_graph), + goal_stages=( + GoalStage( + index=0, + task_id="pick-place", + task_kind="pick_place", + success_state_spec_id="success", + spatial_constraints=(constraint,), + ), + ), + ) + planner = PlannerConfig( + backend=PlannerBackend.SCHEDULESTREAM, + motion_backend=MotionBackend.AUTO, + collisions=True, + max_time_s=10.0, + batch_size=16, + interpolation_dt_s=0.04, + profile=False, + animate=False, + ) + generation = GenerationConfig( + successful_episodes=3, + seed=17, + num_envs=2, + max_attempts=9, + ) + return CompiledTaskRequest( + schema_version=1, + compiler_version="test", + name="franka-pick-cube-into-bowl", + canonical_request_json=canonical_json(canonical_request), + request_digest=sha256_json(canonical_request), + planner=planner, + generation=generation, + output=ResolvedOutputConfig( + dataset=request_directory / "outputs/dataset.hdf5", + keep_failed=False, + run_log=request_directory / "outputs/run_log.jsonl", + ), + arena=arena, + ) + + +def _v2_capabilities() -> CuroboRuntimeCapabilities: + return CuroboRuntimeCapabilities( + api_generation=CuroboApiGeneration.V2, + curobo=DistributionIdentity("nvidia-curobo", "2.0.0", source_commit="curobo-test-commit"), + schedulestream=DistributionIdentity( + "schedulestream", + "0.1.0", + source_commit="schedulestream-test-commit", + ), + has_schedulestream_v1=False, + has_schedulestream_v2=True, + missing_v1_markers=("curobo.wrap.reacher.motion_gen",), + missing_v2_markers=(), + missing_schedulestream_v1_markers=("schedulestream.applications.custream.example",), + missing_schedulestream_v2_markers=(), + ) + + +def _run( + request_path: Path, + *arguments: str, + compiler: Callable[[Path], Any] | None = None, + capability_detector: Callable[[], Any] | None = None, +) -> tuple[int, str, str]: + resolved = _resolved_request(request_path.parent) + stdout = io.StringIO() + stderr = io.StringIO() + result = run_cli( + [str(request_path), *arguments], + compiler=compiler or (lambda _path: resolved), + capability_detector=capability_detector, + backend_selector=select_schedulestream_backend, + stdout=stdout, + stderr=stderr, + ) + return result, stdout.getvalue(), stderr.getvalue() + + +def test_module_import_does_not_load_simulator_or_planner_stacks() -> None: + forbidden = ("isaaclab", "isaaclab_arena", "isaacsim", "schedulestream", "curobo", "torch") + program = ( + "import json, sys; " + "import isaac_autodata_examples.compile_task_request; " + f"print(json.dumps([name for name in {forbidden!r} if name in sys.modules]))" + ) + completed = subprocess.run( + [sys.executable, "-c", program], + check=False, + capture_output=True, + text=True, + ) + assert completed.returncode == 0, completed.stderr + assert json.loads(completed.stdout) == [] + + +def test_dry_run_does_not_import_runtime_probe_or_heavy_stacks() -> None: + forbidden = ( + "isaac_autodata_interfaces.motion_planners.curobo.backend_selection", + "isaaclab", + "isaaclab_arena", + "isaacsim", + "schedulestream", + "curobo", + "torch", + ) + program = f""" +import io +import json +import sys +from isaac_autodata_examples.compile_task_request import run_cli + +class Resolved: + digest = "resolved-digest" + + def to_dict(self): + return {{"name": "test"}} + +stdout = io.StringIO() +stderr = io.StringIO() +code = run_cli( + ["unused.yaml", "--dry-run", "--json"], + compiler=lambda _path: Resolved(), + capability_detector=lambda: (_ for _ in ()).throw(AssertionError("probe called")), + stdout=stdout, + stderr=stderr, +) +print(json.dumps({{"code": code, "forbidden": [name for name in {forbidden!r} if name in sys.modules]}})) +""" + completed = subprocess.run( + [sys.executable, "-c", program], + check=False, + capture_output=True, + text=True, + ) + + assert completed.returncode == 0, completed.stderr + result = json.loads(completed.stdout) + assert result == {"code": 0, "forbidden": []} + + +def test_direct_script_invocation_bootstraps_repository_imports(tmp_path: Path) -> None: + repository_root = Path(__file__).resolve().parents[3] + script = repository_root / "isaac_autodata_examples/compile_task_request.py" + completed = subprocess.run( + [sys.executable, str(script), str(tmp_path / "missing.yaml"), "--dry-run", "--json"], + cwd=tmp_path, + check=False, + capture_output=True, + text=True, + ) + + assert completed.returncode == ExitCode.REQUEST_COMPILATION_FAILED + assert completed.stdout == "" + error = json.loads(completed.stderr)["error"] + assert error["details"]["issues"][0]["code"] == "file_not_found" + + +def test_dry_run_emits_resolved_json_and_skips_capability_probe(tmp_path: Path) -> None: + def fail_if_called() -> Any: + raise AssertionError("dry-run must not probe the runtime") + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--dry-run", + "--json", + capability_detector=fail_if_called, + ) + + assert exit_code == ExitCode.SUCCESS + assert stderr == "" + result = json.loads(stdout) + assert result["mode"] == "dry_run" + assert result["compiled_task"]["planner"]["motion_backend"] == "auto" + assert result["runtime_preflight"] == {"reason": "dry_run_requested", "status": "skipped"} + assert result["execution"] == {"dataset_generated": False, "simulation_launched": False} + + +def test_runtime_preflight_occurs_after_compilation_and_selects_custream2(tmp_path: Path) -> None: + events: list[str] = [] + resolved = _resolved_request(tmp_path) + + def compiler(_path: Path) -> CompiledTaskRequest: + events.append("compile") + return resolved + + def detector() -> CuroboRuntimeCapabilities: + events.append("detect") + return _v2_capabilities() + + stdout = io.StringIO() + stderr = io.StringIO() + exit_code = run_cli( + [str(tmp_path / "request.yaml"), "--json"], + compiler=compiler, + capability_detector=detector, + backend_selector=select_schedulestream_backend, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.SUCCESS + assert events == ["compile", "detect"] + assert stderr.getvalue() == "" + report = json.loads(stdout.getvalue())["runtime_preflight"] + assert report["status"] == "passed" + assert report["requested_motion_backend"] == "auto" + assert report["selected_motion_backend"] == "curobo_v2" + assert report["schedulestream_application"] == "custream2" + assert report["capabilities"]["curobo"]["source_commit"] == "curobo-test-commit" + + +def test_human_output_makes_non_execution_boundary_explicit(tmp_path: Path) -> None: + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + capability_detector=_v2_capabilities, + ) + + assert exit_code == ExitCode.SUCCESS + assert stderr == "" + assert "Runtime preflight: passed" in stdout + assert "Selected motion backend: curobo_v2" in stdout + assert "ScheduleStream application: custream2" in stdout + assert "Simulation launched: no." in stdout + assert "Dataset generated: no." in stdout + + +def test_validation_error_is_structured_and_returns_exit_three(tmp_path: Path) -> None: + issue = ValidationIssue(("planner", "motion_backend"), "invalid_enum", "unsupported backend") + + def compiler(_path: Path) -> Any: + raise AutonomousValidationError([issue]) + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--json", + compiler=compiler, + capability_detector=_v2_capabilities, + ) + + assert exit_code == ExitCode.REQUEST_COMPILATION_FAILED + assert stdout == "" + error = json.loads(stderr)["error"] + assert error["category"] == "request_compilation" + assert error["exit_code"] == 3 + assert error["details"]["issues"] == [ + {"code": "invalid_enum", "message": "unsupported backend", "path": "$.planner.motion_backend"} + ] + + +def test_incompatible_runtime_is_actionable_and_returns_exit_four(tmp_path: Path) -> None: + unavailable = CuroboRuntimeCapabilities( + api_generation=CuroboApiGeneration.UNAVAILABLE, + curobo=None, + schedulestream=None, + has_schedulestream_v1=False, + has_schedulestream_v2=False, + missing_v1_markers=("curobo.wrap.reacher.motion_gen",), + missing_v2_markers=("curobo.motion_planner",), + missing_schedulestream_v1_markers=("schedulestream.applications.custream.example",), + missing_schedulestream_v2_markers=("schedulestream.applications.custream2.policy",), + ) + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--json", + "--write-compiled", + "must-not-exist.json", + capability_detector=lambda: unavailable, + ) + + assert exit_code == ExitCode.RUNTIME_PREFLIGHT_FAILED + assert stdout == "" + error = json.loads(stderr)["error"] + assert error["category"] == "runtime_preflight" + assert error["code"] == "backend_incompatible" + assert error["exit_code"] == 4 + assert error["details"]["requested_motion_backend"] == "auto" + assert "pinned runtime" in error["details"]["remediation"] + assert error["details"]["capabilities"]["api_generation"] == "unavailable" + assert not (tmp_path / "must-not-exist.json").exists() + + +def test_writes_exact_canonical_artifact_beneath_request_directory(tmp_path: Path) -> None: + request_path = tmp_path / "request.yaml" + resolved = _resolved_request(tmp_path) + exit_code, stdout, stderr = _run( + request_path, + "--dry-run", + "--json", + "--write-compiled", + "resolved/request.json", + ) + + artifact = tmp_path / "resolved/request.json" + assert exit_code == ExitCode.SUCCESS + assert stderr == "" + assert artifact.read_text(encoding="utf-8") == resolved.canonical_json() + "\n" + assert json.loads(stdout)["compiled_task_path"] == str(artifact) + assert stat.S_IMODE(artifact.stat().st_mode) & 0o077 == 0 + assert stat.S_IMODE(artifact.parent.stat().st_mode) & 0o077 == 0 + assert list(artifact.parent.glob(".autodata-compiled-*.tmp")) == [] + + +@pytest.mark.parametrize( + ("artifact_name", "expected_code"), + [ + ("../escaped.json", "artifact_path_traversal"), + ("/tmp/absolute.json", "artifact_path_absolute"), + ("resolved/request.yaml", "artifact_extension_invalid"), + (".git/request.json", "artifact_path_forbidden"), + ("resolved//request.json", "artifact_path_traversal"), + (r"resolved\request.json", "artifact_path_invalid"), + ], +) +def test_rejects_unsafe_artifact_names(tmp_path: Path, artifact_name: str, expected_code: str) -> None: + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--dry-run", + "--json", + "--write-compiled", + artifact_name, + ) + + assert exit_code == ExitCode.ARTIFACT_WRITE_FAILED + assert stdout == "" + error = json.loads(stderr)["error"] + assert error["code"] == expected_code + assert error["exit_code"] == 5 + + +def test_refuses_artifact_symlink_escape(tmp_path: Path) -> None: + outside = tmp_path / "outside" + outside.mkdir() + (tmp_path / "escape").symlink_to(outside, target_is_directory=True) + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--dry-run", + "--json", + "--write-compiled", + "escape/request.json", + ) + + assert exit_code == ExitCode.ARTIFACT_WRITE_FAILED + assert stdout == "" + assert json.loads(stderr)["error"]["code"] == "artifact_path_unsafe" + assert not (outside / "request.json").exists() + + +def test_refuses_to_replace_final_artifact_symlink(tmp_path: Path) -> None: + outside = tmp_path / "outside.json" + outside.write_text("external\n", encoding="utf-8") + target = tmp_path / "resolved.json" + target.symlink_to(outside) + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--dry-run", + "--json", + "--write-compiled", + target.name, + ) + + assert exit_code == ExitCode.ARTIFACT_WRITE_FAILED + assert stdout == "" + assert json.loads(stderr)["error"]["code"] == "artifact_exists" + assert target.is_symlink() + assert outside.read_text(encoding="utf-8") == "external\n" + + +def test_refuses_to_overwrite_existing_artifact(tmp_path: Path) -> None: + target = tmp_path / "resolved.json" + target.write_text("human-owned\n", encoding="utf-8") + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--dry-run", + "--json", + "--write-compiled", + target.name, + ) + + assert exit_code == ExitCode.ARTIFACT_WRITE_FAILED + assert stdout == "" + assert json.loads(stderr)["error"]["code"] == "artifact_exists" + assert target.read_text(encoding="utf-8") == "human-owned\n" + + +def test_unexpected_compiler_failure_is_bounded_without_traceback(tmp_path: Path) -> None: + def compiler(_path: Path) -> Any: + raise RuntimeError("first line\nsecond line") + + exit_code, stdout, stderr = _run( + tmp_path / "request.yaml", + "--json", + compiler=compiler, + capability_detector=_v2_capabilities, + ) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + assert "Traceback" not in stderr + error = json.loads(stderr)["error"] + assert error["exit_code"] == 6 + assert error["message"] == "Unexpected RuntimeError: first line second line" diff --git a/isaac_autodata_tests/examples/autonomous/test_generate_task_dataset.py b/isaac_autodata_tests/examples/autonomous/test_generate_task_dataset.py new file mode 100644 index 0000000..d73f356 --- /dev/null +++ b/isaac_autodata_tests/examples/autonomous/test_generate_task_dataset.py @@ -0,0 +1,1487 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import asyncio +import inspect +import io +import json +import os +import signal +import subprocess +import sys +from contextlib import suppress +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import pytest + +from isaac_autodata_examples.generate_task_dataset import ( + ExitCode, + RuntimeStack, + _run_gui_generation_inline, + _run_runtime_child_process, + _RuntimeChildProcessResult, + build_argument_parser, + run_cli, +) +from isaac_autodata_interfaces.autonomous.errors import AutonomousValidationError, ValidationIssue +from isaac_autodata_interfaces.autonomous.runtime_support import validate_runtime_support + + +class _Capabilities: + def to_dict(self) -> dict[str, Any]: + return { + "api_generation": "v1", + "curobo": {"distribution": "nvidia-curobo", "version": "0.7.7"}, + "schedulestream": {"distribution": "schedulestream", "version": "0.1.0"}, + } + + +class _ResolvedRequest: + def __init__(self, root: Path) -> None: + self.name = "franka-pick-cube-into-bowl" + self.request_digest = "b" * 64 + self.digest = "a" * 64 + self.graph_digest = "c" * 64 + self.environment_name = "Agentic-Franka-Pick-Cube-Into-Bowl-v0" + self.planner = SimpleNamespace( + animate=False, + backend=SimpleNamespace(value="schedulestream"), + batch_size=32, + collisions=True, + interpolation_dt_s=0.02, + max_time_s=10.0, + motion_backend=SimpleNamespace(value="auto"), + profile=False, + ) + self.generation = SimpleNamespace( + successful_episodes=3, + max_attempts=7, + num_envs=1, + seed=41, + ) + self.output = SimpleNamespace( + dataset=root / "outputs" / "dataset.hdf5", + keep_failed=False, + run_log=root / "outputs" / "run_log.jsonl", + ) + self.linked_graph = { + "env_name": self.environment_name, + "nodes": [ + {"id": "table", "name": "maple_table_robolab", "params": {}, "type": "background"}, + {"id": "robot", "name": "franka_ik", "params": {}, "type": "embodiment"}, + { + "id": "cube", + "name": "rubiks_cube_hot3d_robolab", + "params": {}, + "type": "object", + }, + {"id": "bowl", "name": "bowl_ycb_robolab", "params": {}, "type": "object"}, + ], + "state_specs": [ + { + "id": "initial", + "is_delta": False, + "spatial_constraints": [ + { + "id": "table_anchor", + "kind": "is_anchor", + "params": {}, + "subject": "table", + }, + { + "id": "cube_on_table", + "kind": "on", + "params": {}, + "reference": "table", + "subject": "cube", + }, + { + "id": "bowl_on_table", + "kind": "on", + "params": {}, + "reference": "table", + "subject": "bowl", + }, + ], + "task_constraints": [], + }, + { + "id": "success", + "is_delta": True, + "spatial_constraints": [{ + "id": "cube_on_bowl", + "kind": "on", + "params": {}, + "reference": "bowl", + "subject": "cube", + }], + "task_constraints": [], + }, + ], + "tasks": [{ + "id": "pick_and_place", + "initial_state_spec_id": "initial", + "kind": "PickAndPlaceTask", + "params": { + "background_scene": "table", + "destination_location": "bowl", + "pick_up_object": "cube", + }, + "success_state_spec_id": "success", + }], + } + self.goal_stages = ( + SimpleNamespace( + index=0, + task_id="pick_and_place", + task_kind="PickAndPlaceTask", + success_state_spec_id="success", + spatial_constraints=( + SimpleNamespace( + id="cube_on_bowl", + kind="on", + params={}, + reference="bowl", + subject="cube", + ), + ), + ), + ) + + def to_dict(self) -> dict[str, Any]: + return { + "arena": { + "graph_digest": self.graph_digest, + "linked_graph": self.linked_graph, + }, + "generation": { + "successful_episodes": self.generation.successful_episodes, + "max_attempts": self.generation.max_attempts, + "num_envs": self.generation.num_envs, + "seed": self.generation.seed, + }, + "name": self.name, + "output": { + "dataset": str(self.output.dataset), + "keep_failed": self.output.keep_failed, + "run_log": str(self.output.run_log), + }, + "request_digest": self.request_digest, + } + + +class _Summary: + def __init__(self, *, target_reached: bool = True) -> None: + self.target_reached = target_reached + + def to_dict(self) -> dict[str, Any]: + return { + "attempts": 4, + "failures": 1, + "last_attempt_id": "attempt-000003", + "request_digest": "b" * 64, + "requested_successful_episodes": 3, + "stop_reason": "requested_successes" if self.target_reached else "max_attempts", + "successes": 3 if self.target_reached else 2, + "target_reached": self.target_reached, + } + + +class _Artifact: + def to_dict(self) -> dict[str, Any]: + return { + "episode_count": 3, + "kind": "successful", + "path": "/attested/dataset.hdf5", + "sha256": "e" * 64, + "size_bytes": 4096, + } + + +class _OutputTransaction: + def __init__(self, harness: _RuntimeHarness) -> None: + self._harness = harness + self.recording_targets = SimpleNamespace( + dataset_export_dir_path="/proc/self/fd/123", + dataset_filename=harness.resolved.output.dataset.stem, + ) + + def open_run_log_writer(self, writer_factory: Any) -> Any: + return writer_factory(self._harness.resolved.output.run_log) + + def publish(self, **kwargs: Any) -> tuple[_Artifact, ...]: + self._harness.events.append("publish:outputs") + self._harness.publish_kwargs = kwargs + if self._harness.publish_error is not None: + raise self._harness.publish_error + commit_record = kwargs["commit_record"] + if commit_record is not None: + kwargs["run_log_writer"].append({**commit_record, "artifacts": [_Artifact().to_dict()]}) + return (_Artifact(),) + + def close(self) -> None: + return + + +class _Resource: + def __init__(self, label: str, events: list[str], *, close_error: Exception | None = None) -> None: + self._label = label + self._events = events + self._close_error = close_error + + def close(self) -> None: + self._events.append(f"close:{self._label}") + if self._close_error is not None: + raise self._close_error + + +class _EventStream(io.StringIO): + def __init__(self, label: str, events: list[str]) -> None: + super().__init__() + self._label = label + self._events = events + + def write(self, value: str) -> int: + self._events.append(f"emit:{self._label}") + return super().write(value) + + def flush(self) -> None: + self._events.append(f"flush:{self._label}") + super().flush() + + +class _RuntimeHarness: + def __init__(self, resolved: _ResolvedRequest) -> None: + self.resolved = resolved + self.events: list[str] = [] + self.app_options: dict[str, Any] | None = None + self.arena_args: Any | None = None + self.generation_kwargs: dict[str, Any] | None = None + self.run_close: bool | None = None + self.run_log_records: list[dict[str, Any]] = [] + self.run_log_append_error: Exception | None = None + self.run_log_append_error_record_type: str | None = None + self.publish_kwargs: dict[str, Any] | None = None + self.publish_error: Exception | None = None + self.attachment_state = object() + self.runtime_attachment_state: Any | None = None + self.executor_attachment_state: Any | None = None + self.planner_attachment_state: Any | None = None + self.summary = _Summary() + self.runtime_error: Exception | None = None + self.run_error: BaseException | None = None + self.close_errors: dict[str, Exception] = {} + + def compile(self, path: Path) -> _ResolvedRequest: + self.events.append("compile") + assert path.name == "request.yaml" + return self.resolved + + def detect(self) -> _Capabilities: + self.events.append("detect") + return _Capabilities() + + def select(self, requested: str, capabilities: _Capabilities) -> Any: + self.events.append("select") + assert requested == "auto" + assert isinstance(capabilities, _Capabilities) + return SimpleNamespace( + capabilities=capabilities, + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + def load_app_launcher(self) -> Any: + self.events.append("load:app_launcher") + + def launch(options: dict[str, Any]) -> Any: + self.events.append("launch:app") + self.app_options = options + app = _Resource("app", self.events, close_error=self.close_errors.get("app")) + return SimpleNamespace(app=app) + + return launch + + def load_runtime_stack(self) -> RuntimeStack: + self.events.append("load:runtime_stack") + return RuntimeStack( + output_transaction_factory=self._reserve_outputs, + arena_runtime_builder=self._build_arena_runtime, + goal_projector=self._project_goal, + attachment_state_factory=self._make_attachment_state, + runtime_factory=self._make_runtime, + success_verifier_factory=self._make_success_verifier, + executor_factory=self._make_executor, + planner_factory=self._make_planner, + run_log_writer_factory=self._make_run_log_writer, + generator_factory=self._make_generator, + generation_request_factory=self._make_generation_request, + run_loop=self._run_loop, + ) + + def _reserve_outputs(self, **kwargs: Any) -> _OutputTransaction: + self.events.append("reserve:outputs") + assert kwargs == { + "dataset_path": self.resolved.output.dataset, + "keep_failed": self.resolved.output.keep_failed, + "run_log_path": self.resolved.output.run_log, + "request_directory": self.resolved.output.dataset.parent.parent, + } + return _OutputTransaction(self) + + def _build_arena_runtime( + self, + resolved: _ResolvedRequest, + args: Any, + *, + recording_targets: Any, + allow_output_overwrite: bool, + ) -> Any: + self.events.append("build:arena_runtime") + assert resolved is self.resolved + assert allow_output_overwrite is False + assert recording_targets.dataset_export_dir_path == "/proc/self/fd/123" + assert recording_targets.dataset_filename == self.resolved.output.dataset.stem + self.arena_args = args + resource = _Resource("arena", self.events, close_error=self.close_errors.get("arena")) + return SimpleNamespace( + close=resource.close, + embodiment_adapter=object(), + env=object(), + success_term=object(), + ) + + def _project_goal(self, resolved: _ResolvedRequest) -> tuple[str, ...]: + self.events.append("project:goal") + assert resolved is self.resolved + return ("cube-inside-bowl",) + + def _make_attachment_state(self) -> object: + self.events.append("create:attachment_state") + return self.attachment_state + + def _make_runtime(self, env: Any, adapter: Any, **kwargs: Any) -> Any: + del env, adapter + self.events.append("create:runtime") + self.runtime_attachment_state = kwargs["attachment_state"] + assert kwargs["graph_nodes"] == tuple(self.resolved.linked_graph["nodes"]) + if self.runtime_error is not None: + raise self.runtime_error + return object() + + def _make_success_verifier(self, success_term: Any) -> Any: + del success_term + self.events.append("create:success_verifier") + return object() + + def _make_executor(self, env: Any, adapter: Any, verifier: Any, **kwargs: Any) -> Any: + del env, adapter, verifier + self.events.append("create:executor") + self.executor_attachment_state = kwargs["attachment_state"] + return object() + + def _make_planner( + self, + bundle: Any, + resolved: _ResolvedRequest, + compatibility: Any, + *, + attachment_state: Any | None = None, + ) -> _Resource: + del bundle + self.events.append("create:planner") + assert resolved is self.resolved + assert compatibility.motion_backend == "curobo_v1" + self.planner_attachment_state = attachment_state + return _Resource("planner", self.events, close_error=self.close_errors.get("planner")) + + def _make_run_log_writer(self, path: Path, **_kwargs: Any) -> Any: + self.events.append("create:run_log_writer") + assert path == self.resolved.output.run_log + + def append(record: dict[str, Any]) -> None: + self.events.append(f"append:{record['record_type']}") + if self.run_log_append_error is not None and record["record_type"] == self.run_log_append_error_record_type: + raise self.run_log_append_error + self.run_log_records.append(record) + + return SimpleNamespace(append=append, close=lambda: None) + + def _make_generator(self, runtime: Any, planner: Any, executor: Any, **kwargs: Any) -> Any: + del runtime, planner, executor + self.events.append("create:generator") + assert kwargs["run_log_writer"] is not None + return object() + + def _make_generation_request(self, **kwargs: Any) -> Any: + self.events.append("create:generation_request") + self.generation_kwargs = kwargs + return SimpleNamespace(**kwargs) + + async def _run_loop(self, generator: Any, request: Any, *, close: bool) -> _Summary: + del generator, request + self.events.append("run:generation") + self.run_close = close + if self.run_error is not None: + raise self.run_error + return self.summary + + +def _run_child( + harness: _RuntimeHarness, + *arguments: str, + expected_digest: str | None = None, + terminal_status_writer: Any | None = None, + stdout: io.StringIO | None = None, + stderr: io.StringIO | None = None, +) -> tuple[int, str, str]: + stdout = stdout or io.StringIO() + stderr = stderr or io.StringIO() + exit_code = run_cli( + [ + str(harness.resolved.output.dataset.parent.parent / "request.yaml"), + "--runtime-child", + "--expected-compiled-task-digest", + expected_digest or harness.resolved.digest, + "--expected-motion-backend", + "curobo_v1", + "--expected-schedulestream-application", + "custream", + "--expected-runtime-support-digest", + _runtime_support_digest(harness.resolved), + "--json", + *arguments, + ], + compiler=harness.compile, + capability_detector=harness.detect, + backend_selector=harness.select, + app_launcher_factory_loader=harness.load_app_launcher, + runtime_stack_loader=harness.load_runtime_stack, + terminal_status_writer=terminal_status_writer or (lambda _status_fd, _exit_code: None), + stdout=stdout, + stderr=stderr, + ) + return exit_code, stdout.getvalue(), stderr.getvalue() + + +def _runtime_support_digest(resolved: _ResolvedRequest) -> str: + return validate_runtime_support( + resolved, + motion_backend="curobo_v1", + schedulestream_application="custream", + ).digest + + +def _run_parent( + harness: _RuntimeHarness, + child_process_runner: Any, + *arguments: str, +) -> tuple[int, str, str]: + stdout = io.StringIO() + stderr = io.StringIO() + exit_code = run_cli( + [str(harness.resolved.output.dataset.parent.parent / "request.yaml"), "--json", *arguments], + compiler=harness.compile, + capability_detector=harness.detect, + backend_selector=harness.select, + child_process_runner=child_process_runner, + stdout=stdout, + stderr=stderr, + ) + return exit_code, stdout.getvalue(), stderr.getvalue() + + +def test_module_import_does_not_load_runtime_or_simulator_stacks() -> None: + forbidden_prefixes = ( + "isaac_autodata_core.autonomous", + "isaac_autodata_interfaces.autonomous.arena_environment", + "isaac_autodata_interfaces.autonomous.isaaclab_runtime", + "isaac_autodata_interfaces.autonomous.schedulestream", + "isaaclab", + "isaaclab_arena", + "isaacsim", + "schedulestream", + "curobo", + "torch", + ) + program = f""" +import json +import sys +import isaac_autodata_examples.generate_task_dataset +print(json.dumps(sorted( + name for name in sys.modules + if any(name == prefix or name.startswith(prefix + '.') for prefix in {forbidden_prefixes!r}) +))) +""" + completed = subprocess.run( + [sys.executable, "-c", program], + check=False, + capture_output=True, + text=True, + ) + + assert completed.returncode == 0, completed.stderr + assert json.loads(completed.stdout) == [] + + +def test_display_mode_defaults_headless_and_exposes_gui() -> None: + parser = build_argument_parser() + + assert parser.parse_args(["request.yaml"]).headless is True + assert parser.parse_args(["request.yaml", "--headless"]).headless is True + assert parser.parse_args(["request.yaml", "--gui"]).headless is False + assert parser.parse_args(["request.yaml", "--no-headless"]).headless is False + + help_text = parser.format_help() + assert "--gui" in help_text + assert "--headless" in help_text + assert "--no-headless" not in help_text + + +@pytest.mark.parametrize( + "display_options", + [ + ("--gui", "--headless"), + ("--gui", "--no-headless"), + ("--headless", "--no-headless"), + ], +) +def test_display_modes_are_mutually_exclusive(display_options: tuple[str, str]) -> None: + with pytest.raises(SystemExit) as exc_info: + build_argument_parser().parse_args(["request.yaml", *display_options]) + + assert exc_info.value.code == 2 + + +def test_gui_selects_kit_and_does_not_claim_the_main_thread_event_loop( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + + def reject_asyncio_run(awaitable: Any) -> None: + with suppress(BaseException): + awaitable.close() + raise AssertionError("GUI generation must not call asyncio.run") + + monkeypatch.setattr(asyncio, "run", reject_asyncio_run) + exit_code, _stdout, stderr = _run_child(harness, "--gui") + + assert exit_code == ExitCode.SUCCESS + assert stderr == "" + assert harness.app_options == { + "device": "cuda:0", + "enable_cameras": False, + "headless": False, + "visualizer": ["kit"], + } + assert "run:generation" in harness.events + + +def test_gui_inline_generation_returns_without_suspending() -> None: + expected = object() + + async def complete_inline() -> object: + return expected + + assert _run_gui_generation_inline(complete_inline()) is expected + + +def test_gui_inline_generation_rejects_suspension_and_closes_coroutine() -> None: + finalized = False + + async def suspend() -> None: + nonlocal finalized + try: + await asyncio.sleep(0) + finally: + finalized = True + + operation = suspend() + with pytest.raises(RuntimeError, match="GUI generation unexpectedly suspended"): + _run_gui_generation_inline(operation) + + assert finalized + assert inspect.getcoroutinestate(operation) == inspect.CORO_CLOSED + + +def test_parent_preflights_then_launches_an_attested_fresh_child(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + commands: list[tuple[str, ...]] = [] + + def launch_child(command: Any) -> int: + harness.events.append("spawn:runtime_child") + commands.append(tuple(command)) + return int(ExitCode.GENERATION_INCOMPLETE) + + exit_code, stdout, stderr = _run_parent( + harness, + launch_child, + "--device", + "cuda:3", + "--gui", + "--enable-cameras", + ) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + assert stdout == "" + assert stderr == "" + assert harness.events == ["compile", "detect", "select", "spawn:runtime_child"] + assert "load:app_launcher" not in harness.events + command = commands[0] + assert command[:2] == (sys.executable, "-u") + assert Path(command[2]).name == "generate_task_dataset.py" + assert Path(command[3]) == (tmp_path / "request.yaml").resolve() + assert command[command.index("--expected-compiled-task-digest") + 1] == harness.resolved.digest + assert command[command.index("--expected-motion-backend") + 1] == "curobo_v1" + assert command[command.index("--expected-schedulestream-application") + 1] == "custream" + assert command[command.index("--expected-runtime-support-digest") + 1] == _runtime_support_digest(harness.resolved) + assert "--runtime-child" in command + assert "--status-fd" not in command + assert "--no-headless" in command + assert "--enable-cameras" in command + + +def test_parent_uses_terminal_channel_instead_of_false_process_success(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + false_success = _RuntimeChildProcessResult( + process_return_code=0, + status_payload=b'{"exit_code":8,"protocol_version":1}\n', + ) + + exit_code, stdout, stderr = _run_parent(harness, lambda _command: false_success) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + assert stdout == "" + assert stderr == "" + + +def test_parent_rejects_success_status_followed_by_abnormal_process_exit(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + false_success = _RuntimeChildProcessResult( + process_return_code=-9, + status_payload=b'{"exit_code":0,"protocol_version":1}\n', + ) + + exit_code, stdout, stderr = _run_parent(harness, lambda _command: false_success) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + error = json.loads(stderr)["error"] + assert error["code"] == "child_success_process_failed" + assert error["details"] == {"process_return_code": -9} + + +def test_parent_accepts_failure_status_despite_abnormal_process_exit(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + child_failure = _RuntimeChildProcessResult( + process_return_code=-9, + status_payload=b'{"exit_code":8,"protocol_version":1}\n', + ) + + exit_code, stdout, stderr = _run_parent(harness, lambda _command: child_failure) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + assert stdout == "" + assert stderr == "" + + +def test_real_subprocess_runner_passes_private_status_descriptor() -> None: + program = ( + 'import os,sys; fd=int(sys.argv[-1]); os.write(fd,b\'{"exit_code":8,"protocol_version":1}\\n\'); os.close(fd)' + ) + + result = _run_runtime_child_process((sys.executable, "-c", program)) + + assert result.process_return_code == 0 + assert result.status_payload == b'{"exit_code":8,"protocol_version":1}\n' + + +def test_subprocess_runner_timeout_escalates_process_group_and_reaps(monkeypatch: pytest.MonkeyPatch) -> None: + signals: list[int] = [] + popen_options: dict[str, Any] = {} + group_alive = True + + class HungProcess: + pid = 424_201 + returncode: int | None = None + reaped = False + + def poll(self) -> int | None: + return self.returncode + + def wait(self, timeout: float | None = None) -> int: + if signal.SIGKILL not in signals: + raise subprocess.TimeoutExpired(cmd="runtime-child", timeout=timeout) + self.returncode = -signal.SIGKILL + self.reaped = True + return self.returncode + + process = HungProcess() + + def create_process(_command: Any, **options: Any) -> HungProcess: + popen_options.update(options) + return process + + def signal_group(_pid: int, signum: int) -> None: + nonlocal group_alive + if signum == 0: + if not group_alive: + raise ProcessLookupError + return + signals.append(signum) + if signum == signal.SIGKILL: + group_alive = False + + monkeypatch.setattr(subprocess, "Popen", create_process) + monkeypatch.setattr(os, "killpg", signal_group) + + result = _run_runtime_child_process( + (sys.executable, "-c", "pass"), + wall_timeout_s=1e-9, + terminate_grace_s=1e-9, + ) + + assert signals == [signal.SIGTERM, signal.SIGKILL] + assert process.reaped is True + assert result == _RuntimeChildProcessResult(process_return_code=-signal.SIGKILL, status_payload=b"") + assert popen_options["start_new_session"] is True + assert len(popen_options["pass_fds"]) == 1 + + +def test_subprocess_runner_keyboard_interrupt_kills_group_and_prevents_orphan( + monkeypatch: pytest.MonkeyPatch, +) -> None: + signals: list[int] = [] + inherited_status_fd: int | None = None + group_alive = True + + class InterruptedProcess: + pid = 424_202 + returncode: int | None = None + reaped = False + + def poll(self) -> int | None: + return self.returncode + + def wait(self, timeout: float | None = None) -> int: + if not signals: + raise KeyboardInterrupt + self.returncode = -signal.SIGINT + self.reaped = True + return self.returncode + + process = InterruptedProcess() + + def create_process(command: Any, **_options: Any) -> InterruptedProcess: + nonlocal inherited_status_fd + inherited_status_fd = int(command[-1]) + return process + + def signal_group(_pid: int, signum: int) -> None: + nonlocal group_alive + if signum == 0: + if not group_alive: + raise ProcessLookupError + return + signals.append(signum) + group_alive = False + + monkeypatch.setattr(subprocess, "Popen", create_process) + monkeypatch.setattr(os, "killpg", signal_group) + + with pytest.raises(KeyboardInterrupt): + _run_runtime_child_process( + (sys.executable, "-c", "pass"), + wall_timeout_s=10.0, + terminate_grace_s=0.1, + ) + + assert signals == [signal.SIGINT] + assert process.reaped is True + assert inherited_status_fd is not None + with pytest.raises(OSError): + os.fstat(inherited_status_fd) + + +def test_subprocess_runner_forwards_sigterm_to_child_process_group(monkeypatch: pytest.MonkeyPatch) -> None: + installed_handlers: dict[int, Any] = {} + signals: list[int] = [] + group_alive = True + + class TerminatedProcess: + pid = 424_203 + returncode: int | None = None + reaped = False + + def poll(self) -> int | None: + return self.returncode + + def wait(self, timeout: float | None = None) -> int: + del timeout + installed_handlers[signal.SIGTERM](signal.SIGTERM, None) + self.returncode = -signal.SIGTERM + self.reaped = True + return self.returncode + + process = TerminatedProcess() + + def install_handler(signum: int, handler: Any) -> None: + installed_handlers[signum] = handler + + def signal_group(_pid: int, signum: int) -> None: + nonlocal group_alive + if signum == 0: + if not group_alive: + raise ProcessLookupError + return + signals.append(signum) + group_alive = False + + monkeypatch.setattr(subprocess, "Popen", lambda *_args, **_kwargs: process) + monkeypatch.setattr(os, "killpg", signal_group) + monkeypatch.setattr(signal, "signal", install_handler) + + with pytest.raises(KeyboardInterrupt): + _run_runtime_child_process( + (sys.executable, "-c", "pass"), + wall_timeout_s=10.0, + terminate_grace_s=0.1, + ) + + assert signals == [signal.SIGTERM] + assert process.reaped is True + + +def test_subprocess_runner_kills_descendant_that_outlives_successful_direct_child(tmp_path: Path) -> None: + descendant_record = tmp_path / "descendant.txt" + descendant_program = ( + "import os,signal,time; signal.signal(signal.SIGTERM,signal.SIG_IGN); os.write(1,b'R'); time.sleep(60)" + ) + direct_program = ( + "import os,pathlib,subprocess,sys; " + f"child=subprocess.Popen([sys.executable,'-c',{descendant_program!r}],stdout=subprocess.PIPE); " + "assert child.stdout.read(1)==b'R'; " + "pathlib.Path(sys.argv[1]).write_text(f'{child.pid} {os.getpgid(child.pid)}',encoding='utf-8'); " + "fd=int(sys.argv[-1]); " + 'os.write(fd,b\'{"exit_code":0,"protocol_version":1}\\n\'); ' + "os.close(fd)" + ) + + try: + with pytest.raises(RuntimeError, match="descendants remained"): + _run_runtime_child_process( + (sys.executable, "-c", direct_program, str(descendant_record)), + wall_timeout_s=5.0, + terminate_grace_s=0.25, + ) + finally: + if descendant_record.exists(): + descendant_pid = int(descendant_record.read_text(encoding="utf-8").split()[0]) + with suppress(ProcessLookupError): + os.kill(descendant_pid, signal.SIGKILL) + + +@pytest.mark.parametrize( + ("payload", "expected_code"), + [ + (b"", "child_status_missing"), + (b"not-json\n", "child_status_malformed"), + (b'{"exit_code":true,"protocol_version":1}\n', "child_status_malformed"), + (b'{"exit_code":0,"protocol_version":99}\n', "child_status_malformed"), + (b'{"exit_code":0,"exit_code":8,"protocol_version":1}\n', "child_status_malformed"), + (b"x" * 513, "child_status_malformed"), + ], +) +def test_parent_refuses_missing_or_malformed_terminal_status( + tmp_path: Path, + payload: bytes, + expected_code: str, +) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + untrusted_success = _RuntimeChildProcessResult( + process_return_code=0, + status_payload=payload, + ) + + exit_code, stdout, stderr = _run_parent(harness, lambda _command: untrusted_success) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + error = json.loads(stderr)["error"] + assert error["category"] == "runtime_handoff" + assert error["code"] == expected_code + assert error["details"] == { + "process_return_code": 0, + "status_bytes": len(payload), + } + assert "Traceback" not in stderr + + +def test_runtime_child_launches_app_before_compile_and_refuses_digest_mismatch( + tmp_path: Path, +) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + + exit_code, stdout, stderr = _run_child(harness, expected_digest="d" * 64) + + assert exit_code == ExitCode.RUNTIME_SETUP_FAILED + assert stdout == "" + assert harness.events == ["load:app_launcher", "launch:app", "compile", "close:app"] + error = json.loads(stderr)["error"] + assert error["category"] == "runtime_handoff" + assert error["code"] == "compiled_task_digest_mismatch" + assert error["details"] == { + "actual_compiled_task_digest": harness.resolved.digest, + "expected_compiled_task_digest": "d" * 64, + } + + +def test_happy_path_wires_runtime_in_order_and_closes_owned_resources(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + + exit_code, stdout, stderr = _run_child( + harness, + "--device", + "cuda:2", + "--enable-cameras", + ) + + assert exit_code == ExitCode.SUCCESS + assert stderr == "" + result = json.loads(stdout) + assert result["status"] == "completed" + assert result["generation"]["target_reached"] is True + assert result["backend"]["selected_motion_backend"] == "curobo_v1" + assert harness.events == [ + "load:app_launcher", + "launch:app", + "compile", + "detect", + "select", + "load:runtime_stack", + "reserve:outputs", + "create:run_log_writer", + "append:run_started", + "build:arena_runtime", + "project:goal", + "create:attachment_state", + "create:runtime", + "create:success_verifier", + "create:executor", + "create:planner", + "create:generator", + "create:generation_request", + "run:generation", + "append:run_summary", + "close:planner", + "close:arena", + "publish:outputs", + "append:run_committed", + "close:app", + ] + assert harness.app_options == {"device": "cuda:2", "enable_cameras": True, "headless": True} + assert harness.arena_args.device == "cuda:2" + assert harness.arena_args.placement_seed == 41 + assert harness.arena_args.seed == 41 + assert harness.arena_args.solve_relations is True + assert harness.runtime_attachment_state is harness.attachment_state + assert harness.executor_attachment_state is harness.attachment_state + assert harness.planner_attachment_state is harness.attachment_state + assert harness.generation_kwargs == { + "base_seed": 41, + "successful_episodes": 3, + "goal": ("cube-inside-bowl",), + "keep_failed": False, + "max_attempts": 7, + "num_envs": 1, + "request_digest": "b" * 64, + "expected_plan_backend": "schedulestream_custream", + } + assert harness.run_close is False + assert [record["record_type"] for record in harness.run_log_records] == [ + "run_started", + "run_summary", + "run_committed", + ] + assert harness.run_log_records[0]["preflight"]["status"] == "passed" + + +def test_child_emits_flushes_and_publishes_status_before_app_close(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + stdout = _EventStream("stdout", harness.events) + stderr = _EventStream("stderr", harness.events) + + def publish_status(status_fd: int | None, exit_code: int) -> None: + assert status_fd is None + harness.events.append(f"publish:status:{exit_code}") + + exit_code, _, error_output = _run_child( + harness, + terminal_status_writer=publish_status, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.SUCCESS + assert error_output == "" + emit_index = harness.events.index("emit:stdout") + status_index = harness.events.index("publish:status:0") + app_close_index = harness.events.index("close:app") + assert harness.events.index("close:planner") < emit_index + assert harness.events.index("close:arena") < emit_index + assert harness.events.index("publish:outputs") < emit_index + assert harness.events.index("flush:stdout", emit_index) < status_index + assert harness.events.index("flush:stderr", emit_index) < status_index + assert status_index < app_close_index + assert app_close_index == len(harness.events) - 1 + + +def test_child_marks_status_descriptor_non_inheritable_before_app_launch(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + read_fd, write_fd = os.pipe() + os.set_inheritable(write_fd, True) + + def publish_status(status_fd: int | None, _exit_code: int) -> None: + assert status_fd == write_fd + assert os.get_inheritable(write_fd) is False + + try: + exit_code, _, _ = _run_child( + harness, + "--status-fd", + str(write_fd), + terminal_status_writer=publish_status, + ) + finally: + os.close(write_fd) + os.close(read_fd) + + assert exit_code == ExitCode.SUCCESS + + +def test_child_flushes_failure_status_before_app_close(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.run_error = RuntimeError("execution failed") + stdout = _EventStream("stdout", harness.events) + stderr = _EventStream("stderr", harness.events) + + def publish_status(_status_fd: int | None, exit_code: int) -> None: + harness.events.append(f"publish:status:{exit_code}") + + exit_code, _, _ = _run_child( + harness, + terminal_status_writer=publish_status, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + emit_index = harness.events.index("emit:stderr") + status_index = harness.events.index(f"publish:status:{int(ExitCode.GENERATION_INCOMPLETE)}") + app_close_index = harness.events.index("close:app") + assert harness.events.index("flush:stderr", emit_index) < status_index + assert status_index < app_close_index + assert app_close_index == len(harness.events) - 1 + + +def test_incomplete_summary_is_reported_after_cleanup(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.summary = _Summary(target_reached=False) + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + assert stderr == "" + result = json.loads(stdout) + assert result["status"] == "incomplete" + assert result["generation"]["stop_reason"] == "max_attempts" + assert harness.events[-5:] == [ + "close:planner", + "close:arena", + "publish:outputs", + "append:run_committed", + "close:app", + ] + + +def test_semantic_validation_failure_prevents_probe_and_app_import(tmp_path: Path) -> None: + events: list[str] = [] + issue = ValidationIssue(("generation", "successful_episodes"), "invalid_integer", "must be positive") + + def compiler(_path: Path) -> Any: + events.append("compile") + raise AutonomousValidationError([issue]) + + def forbidden() -> Any: + raise AssertionError("post-compilation dependency must not be called") + + stdout = io.StringIO() + stderr = io.StringIO() + exit_code = run_cli( + [str(tmp_path / "request.yaml"), "--json"], + compiler=compiler, + capability_detector=forbidden, + backend_selector=lambda _requested, _capabilities: forbidden(), + app_launcher_factory_loader=forbidden, + runtime_stack_loader=forbidden, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.REQUEST_COMPILATION_FAILED + assert stdout.getvalue() == "" + assert events == ["compile"] + error = json.loads(stderr.getvalue())["error"] + assert error["details"]["issues"] == [issue.to_dict()] + + +def test_runtime_preflight_failure_prevents_app_import(tmp_path: Path) -> None: + resolved = _ResolvedRequest(tmp_path) + events: list[str] = [] + + def detector() -> Any: + events.append("detect") + raise RuntimeError("probe unavailable\nwithout mutating the host") + + def forbidden() -> Any: + raise AssertionError("AppLauncher must not be imported after failed preflight") + + stdout = io.StringIO() + stderr = io.StringIO() + exit_code = run_cli( + [str(tmp_path / "request.yaml"), "--json"], + compiler=lambda _path: resolved, + capability_detector=detector, + backend_selector=lambda _requested, _capabilities: forbidden(), + app_launcher_factory_loader=forbidden, + runtime_stack_loader=forbidden, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.RUNTIME_PREFLIGHT_FAILED + assert stdout.getvalue() == "" + assert events == ["detect"] + error = json.loads(stderr.getvalue())["error"] + assert error["details"]["requested_motion_backend"] == "auto" + assert error["details"]["capabilities"] == {"status": "probe_failed_before_result"} + assert error["message"] == "probe unavailable without mutating the host" + assert "Traceback" not in stderr.getvalue() + + +def test_malformed_backend_selection_is_structured_and_never_reaches_child(tmp_path: Path) -> None: + resolved = _ResolvedRequest(tmp_path) + stdout = io.StringIO() + stderr = io.StringIO() + + exit_code = run_cli( + [str(tmp_path / "request.yaml"), "--json"], + compiler=lambda _path: resolved, + capability_detector=_Capabilities, + backend_selector=lambda _requested, capabilities: SimpleNamespace( + capabilities=capabilities, + motion_backend="unreviewed_backend", + schedulestream_application="custream", + ), + child_process_runner=lambda _command: (_ for _ in ()).throw(AssertionError("child launched")), + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.RUNTIME_PREFLIGHT_FAILED + assert stdout.getvalue() == "" + error = json.loads(stderr.getvalue())["error"] + assert error["code"] == "backend_incompatible" + assert error["message"] == "backend selector returned no supported motion backend" + + +@pytest.mark.parametrize("occupied_target", ("dataset", "failed_dataset", "run_log")) +def test_existing_output_prevents_app_launch(tmp_path: Path, occupied_target: str) -> None: + resolved = _ResolvedRequest(tmp_path) + if occupied_target == "failed_dataset": + resolved.output.keep_failed = True + dataset_path = resolved.output.dataset + target = dataset_path.with_name(f"{dataset_path.stem}_failed{dataset_path.suffix}") + else: + target = getattr(resolved.output, occupied_target) + target.parent.mkdir(parents=True) + target.write_text("human-owned\n", encoding="utf-8") + harness = _RuntimeHarness(resolved) + + exit_code, stdout, stderr = _run_parent( + harness, + lambda _command: (_ for _ in ()).throw(AssertionError("child launched")), + ) + + assert exit_code == ExitCode.OUTPUT_CONFLICT + assert stdout == "" + assert harness.events == ["compile", "detect", "select"] + assert target.read_text(encoding="utf-8") == "human-owned\n" + assert json.loads(stderr)["error"]["code"] == "output_conflict" + + +def test_runtime_child_rechecks_output_after_digest_attestation(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.resolved.output.dataset.parent.mkdir(parents=True) + harness.resolved.output.dataset.write_text("appeared-after-parent-preflight\n", encoding="utf-8") + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.OUTPUT_CONFLICT + assert stdout == "" + assert harness.events == [ + "load:app_launcher", + "launch:app", + "compile", + "detect", + "select", + "close:app", + ] + assert json.loads(stderr)["error"]["code"] == "output_conflict" + + +def test_app_launch_failure_does_not_load_heavy_runtime_stack(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + + def fail_launch() -> Any: + harness.events.append("load:app_launcher") + + def launcher(_options: dict[str, Any]) -> Any: + harness.events.append("launch:app") + raise RuntimeError("Kit startup failed") + + return launcher + + harness.load_app_launcher = fail_launch # type: ignore[method-assign] + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.APP_LAUNCH_FAILED + assert stdout == "" + assert harness.events[-2:] == ["load:app_launcher", "launch:app"] + assert "load:runtime_stack" not in harness.events + assert "close:" not in " ".join(harness.events) + assert "Traceback" not in stderr + + +def test_runtime_setup_failure_closes_arena_and_app(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.runtime_error = RuntimeError("runtime binding rejected") + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.RUNTIME_SETUP_FAILED + assert stdout == "" + assert harness.events[-3:] == ["close:arena", "append:run_aborted", "close:app"] + assert harness.run_log_records[-1]["record_type"] == "run_aborted" + assert "create:planner" not in harness.events + assert json.loads(stderr)["error"]["details"]["exception_type"] == "RuntimeError" + + +def test_generation_failure_closes_planner_arena_and_app(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.run_error = RuntimeError("attempt execution failed\nwith bounded diagnostics") + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.GENERATION_INCOMPLETE + assert stdout == "" + assert harness.events[-4:] == [ + "close:planner", + "close:arena", + "append:run_aborted", + "close:app", + ] + assert harness.run_log_records[-1]["record_type"] == "run_aborted" + error = json.loads(stderr)["error"] + assert error["category"] == "generation" + assert error["message"].endswith("attempt execution failed with bounded diagnostics") + assert "Traceback" not in stderr + + +def test_publication_failure_is_terminally_recorded_before_status(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.publish_error = ValueError("staged dataset schema rejected") + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + assert harness.events[-4:] == [ + "close:arena", + "publish:outputs", + "append:run_aborted", + "close:app", + ] + assert harness.run_log_records[-1]["record_type"] == "run_aborted" + error = json.loads(stderr)["error"] + assert error["category"] == "output_publication" + assert error["code"] == "dataset_publication_failed" + + +def test_ambiguous_commit_does_not_write_the_ledger_again(tmp_path: Path) -> None: + class DatasetCommitUncertainError(RuntimeError): + pass + + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.publish_error = DatasetCommitUncertainError("ledger append durability is unknown") + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + publish_index = harness.events.index("publish:outputs") + assert not any(event.startswith("append:") for event in harness.events[publish_index + 1 :]) + error = json.loads(stderr)["error"] + assert error["code"] == "dataset_commit_uncertain" + assert "inspect" in error["details"]["recovery"] + + +def test_ambiguous_run_log_commit_does_not_write_the_ledger_again(tmp_path: Path) -> None: + class RunLogWriteUncertainError(RuntimeError): + pass + + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.run_log_append_error = RunLogWriteUncertainError("commit record fsync outcome is unknown") + harness.run_log_append_error_record_type = "run_committed" + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + ambiguous_append_index = harness.events.index("append:run_committed") + assert not any(event.startswith("append:") for event in harness.events[ambiguous_append_index + 1 :]) + error = json.loads(stderr)["error"] + assert error["category"] == "run_log" + assert error["code"] == "run_log_write_uncertain" + assert error["details"]["phase"] == "output_publication" + assert "Inspect its existing JSONL tail" in error["details"]["recovery"] + + +def test_ambiguous_terminal_failure_append_replaces_prior_failure_without_retry(tmp_path: Path) -> None: + class RunLogWriteUncertainError(RuntimeError): + pass + + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.run_error = RuntimeError("generation stopped") + harness.run_log_append_error = RunLogWriteUncertainError("abort record may already be durable") + harness.run_log_append_error_record_type = "run_aborted" + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.INTERNAL_ERROR + assert stdout == "" + ambiguous_append_index = harness.events.index("append:run_aborted") + assert not any(event.startswith("append:") for event in harness.events[ambiguous_append_index + 1 :]) + error = json.loads(stderr)["error"] + assert error["code"] == "run_log_write_uncertain" + assert error["details"]["phase"] == "terminal_run_log" + assert error["details"]["preceding_failure"]["code"] == "generation_failed" + + +def test_cleanup_failure_attempts_all_callbacks_and_returns_exit_nine(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.close_errors = { + "arena": RuntimeError("arena close failed"), + "planner": RuntimeError("planner close failed"), + } + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.CLEANUP_FAILED + assert stdout == "" + assert harness.events[-4:] == [ + "close:planner", + "close:arena", + "append:run_cleanup_failed", + "close:app", + ] + assert harness.run_log_records[-1]["record_type"] == "run_cleanup_failed" + error = json.loads(stderr)["error"] + assert [item["resource"] for item in error["details"]["cleanup_failures"]] == [ + "episode_planner", + "arena_runtime", + ] + assert error["details"]["generation_summary"]["target_reached"] is True + + +def test_operator_interrupt_returns_130_after_cleanup(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + harness.run_error = KeyboardInterrupt() + + exit_code, stdout, stderr = _run_child(harness) + + assert exit_code == ExitCode.INTERRUPTED + assert stdout == "" + assert harness.events[-4:] == [ + "close:planner", + "close:arena", + "append:run_interrupted", + "close:app", + ] + assert harness.run_log_records[-1]["record_type"] == "run_interrupted" + error = json.loads(stderr)["error"] + assert error["category"] == "interrupted" + assert error["details"]["phase"] == "generation" + + +def test_human_summary_contains_terminal_counts_and_output_paths(tmp_path: Path) -> None: + harness = _RuntimeHarness(_ResolvedRequest(tmp_path)) + stdout = io.StringIO() + stderr = io.StringIO() + + exit_code = run_cli( + [ + str(tmp_path / "request.yaml"), + "--runtime-child", + "--expected-compiled-task-digest", + harness.resolved.digest, + "--expected-motion-backend", + "curobo_v1", + "--expected-schedulestream-application", + "custream", + "--expected-runtime-support-digest", + _runtime_support_digest(harness.resolved), + ], + compiler=harness.compile, + capability_detector=harness.detect, + backend_selector=harness.select, + app_launcher_factory_loader=harness.load_app_launcher, + runtime_stack_loader=harness.load_runtime_stack, + terminal_status_writer=lambda _status_fd, _exit_code: None, + stdout=stdout, + stderr=stderr, + ) + + assert exit_code == ExitCode.SUCCESS + assert stderr.getvalue() == "" + assert "Dataset generation completed." in stdout.getvalue() + assert "attempts=4, successes=3, failures=1, requested_successes=3" in stdout.getvalue() + assert str(harness.resolved.output.dataset) in stdout.getvalue() + assert "Cleanup completed: true" in stdout.getvalue() + + +def test_direct_script_compilation_failure_is_structured_without_launching_isaac(tmp_path: Path) -> None: + repository_root = Path(__file__).resolve().parents[3] + script = repository_root / "isaac_autodata_examples/generate_task_dataset.py" + + completed = subprocess.run( + [sys.executable, str(script), str(tmp_path / "missing.yaml"), "--json"], + cwd=tmp_path, + check=False, + capture_output=True, + text=True, + ) + + assert completed.returncode == ExitCode.REQUEST_COMPILATION_FAILED + assert completed.stdout == "" + error = json.loads(completed.stderr)["error"] + assert error["category"] == "request_compilation" + assert error["details"]["issues"][0]["code"] == "file_not_found" diff --git a/isaac_autodata_tests/interfaces/__init__.py b/isaac_autodata_tests/interfaces/__init__.py new file mode 100644 index 0000000..1503874 --- /dev/null +++ b/isaac_autodata_tests/interfaces/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Interface test package.""" diff --git a/isaac_autodata_tests/interfaces/autonomous/__init__.py b/isaac_autodata_tests/interfaces/autonomous/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/__init__.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/__init__.py new file mode 100644 index 0000000..9468998 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/__init__.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/__init__.py new file mode 100644 index 0000000..8975efb --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for experimental ScheduleStream integration surfaces.""" diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/test_command_lowering.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/test_command_lowering.py new file mode 100644 index 0000000..ee4788d --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/experimental/test_command_lowering.py @@ -0,0 +1,370 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import json +import subprocess +import sys +from types import SimpleNamespace + +import pytest + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + ConcurrentGroupSegment, + DetachIntentSegment, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + JointTrajectorySegment, + WaitSegment, +) +from isaac_autodata_interfaces.autonomous.schedulestream.command_types import ( + MalformedScheduleStreamCommandError, + ScheduleStreamClosedError, + ScheduleStreamCommandSymbols, + ScheduleStreamLoweringContext, + ScheduleStreamTimingError, + UnsupportedScheduleStreamCommandError, +) +from isaac_autodata_interfaces.autonomous.schedulestream.experimental.command_lowering import ( + ScheduleStreamCommandLowerer, +) + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) +TRANSLATED = ( + (1.0, 0.0, 0.0, 0.2), + (0.0, 1.0, 0.0, -0.1), + (0.0, 0.0, 1.0, 0.3), + (0.0, 0.0, 0.0, 1.0), +) + + +class _World: + def __init__(self, dt: float = 0.05, arm_by_link: dict[str, str] | None = None) -> None: + self.time_step = dt + self.arm_by_link = arm_by_link or {"tool": "arm"} + + def get_link_arm(self, link: str) -> str: + return self.arm_by_link[link] + + +class _Command: + def __init__(self, world: _World) -> None: + self.world = world + + @property + def time_step(self) -> float: + return self.world.time_step + + +class _Commands(_Command): + def __init__(self, world: _World, commands: list[object]) -> None: + super().__init__(world) + self.commands = commands + + +class _Composite(_Commands): + pass + + +class _JointState: + def __init__(self, names: tuple[str, ...], positions: list[list[float]]) -> None: + self.joint_names = names + self.position = positions + + +class _Configuration(_Command): + def __init__(self, world: _World, names: tuple[str, ...], position: list[float]) -> None: + super().__init__(world) + self.joint_state = _JointState(names, [position]) + + @property + def joints(self) -> list[str]: + return list(self.joint_state.joint_names) + + +class _Trajectory(_Command): + def __init__(self, world: _World, names: tuple[str, ...], positions: list[list[float]]) -> None: + super().__init__(world) + self.joint_state = _JointState(names, positions) + + @property + def joints(self) -> list[str]: + return list(self.joint_state.joint_names) + + +class _V1LinkPath(_Command): + def __init__(self, world: _World, arm: str, link: str, poses: list[object]) -> None: + super().__init__(world) + self.arm = arm + self.link = link + self.poses = poses + + def __len__(self) -> int: + return len(self.poses) + + def __getitem__(self, index: int) -> object: + return self.poses[index] + + +class _V2LinkPath(_Command): + def __init__(self, world: _World, arm: str, link: str, poses: list[object]) -> None: + super().__init__(world) + self.arm = arm + self.link = link + self.poses = poses + + @property + def length(self) -> int: + return len(self.poses) + + def pose(self, index: int) -> object: + return self.poses[index] + + +class _Open(_Command): + def __init__(self, world: _World, arm: str, num_steps: int = 1) -> None: + super().__init__(world) + self.arm = arm + self.num_steps = num_steps + + +class _Close(_Open): + pass + + +class _Attach(_Command): + def __init__( + self, + world: _World, + obj: str, + *, + arm: str | None, + parent: str | None = None, + link: str | None = None, + ) -> None: + super().__init__(world) + self.obj = obj + self.arm = arm + self.parent = parent + self.link = link + self.num_steps = 1 + + +class _Detach(_Command): + def __init__( + self, + world: _World, + *, + arm: str | None, + parent: str | None = None, + link: str | None = None, + ) -> None: + super().__init__(world) + self.arm = arm + self.parent = parent + self.link = link + self.num_steps = 1 + + +def _symbols(application: str) -> ScheduleStreamCommandSymbols: + return ScheduleStreamCommandSymbols( + application=application, + commands_type=_Commands, + composite_type=_Composite, + configuration_type=_Configuration, + trajectory_type=_Trajectory, + link_path_type=_V1LinkPath if application == "custream" else _V2LinkPath, + open_type=_Open, + close_type=_Close, + attach_type=_Attach, + detach_type=_Detach, + pose_to_matrix=lambda pose: pose, + ) + + +def _selector(application: str): + motion_backend = "curobo_v1" if application == "custream" else "curobo_v2" + return lambda requested, capabilities: SimpleNamespace( + motion_backend=motion_backend, + schedulestream_application=application, + capabilities=SimpleNamespace(schedulestream=SimpleNamespace(version="test", source_commit="abcdef0123456789")), + ) + + +def _context(**kwargs) -> ScheduleStreamLoweringContext: + values = { + "request_digest": "request", + "snapshot_digest": "snapshot", + "seed": 7, + "goal": (GoalPredicate("on", "cube", "table"),), + "eef_name": "hand", + "frame": "world", + "step_dt_s": 0.05, + } + values.update(kwargs) + return ScheduleStreamLoweringContext(**values) + + +def _boundary(application: str, **kwargs) -> ScheduleStreamCommandLowerer: + return ScheduleStreamCommandLowerer( + selector=_selector(application), + symbols=_symbols(application), + **kwargs, + ) + + +def test_import_is_free_of_schedulestream_and_curobo_modules() -> None: + forbidden = ("curobo", "schedulestream") + program = ( + "import json,sys; import isaac_autodata_interfaces.autonomous.schedulestream; " + f"print(json.dumps([name for name in {forbidden!r} if name in sys.modules]))" + ) + completed = subprocess.run([sys.executable, "-c", program], check=False, capture_output=True, text=True) + + assert completed.returncode == 0, completed.stderr + assert json.loads(completed.stdout) == [] + + +def test_v1_lowers_every_supported_sequential_command_without_dropping() -> None: + world = _World() + commands = _Commands( + world, + [ + _Open(world, "arm", num_steps=2), + _V1LinkPath(world, "arm", "tool", [IDENTITY, TRANSLATED]), + _Attach(world, "native_cube", arm="arm", parent="tool"), + _Configuration(world, ("j1", "j2"), [0.1, 0.2]), + _Configuration(world, ("j1", "j2"), [0.1, 0.2]), + _Detach(world, arm="arm", parent="tool"), + _Close(world, "arm", num_steps=3), + _Trajectory(world, ("j1", "j2"), [[0.1, 0.2], [0.2, 0.3]]), + ], + ) + + plan = _boundary("custream").lower( + commands, + _context(object_name_map={"native_cube": "cube"}), + ) + + assert [type(segment) for segment in plan.segments] == [ + GripperCommandSegment, + CartesianTrajectorySegment, + AttachIntentSegment, + WaitSegment, + DetachIntentSegment, + GripperCommandSegment, + JointTrajectorySegment, + ] + assert plan.segments[0].command is GripperCommandMode.OPEN + assert plan.segments[0].settle_steps == 2 + assert plan.segments[2].object_name == "cube" + assert plan.segments[3].steps == 2 + assert plan.segments[4].object_name == "cube" + assert plan.segments[5].command is GripperCommandMode.CLOSE + assert plan.segments[-1].timestamps_s == (0.0, 0.05) + assert plan.metadata["schedulestream"]["command_nodes"] == 9 + + +def test_v2_recursively_preserves_disjoint_composite_branches() -> None: + world = _World(arm_by_link={"left_tool": "left", "right_tool": "right"}) + composite = _Composite( + world, + [ + _V2LinkPath(world, "left", "left_tool", [IDENTITY, TRANSLATED]), + _Commands( + world, + [ + _Open(world, "right"), + _V2LinkPath(world, "right", "right_tool", [IDENTITY, TRANSLATED]), + ], + ), + ], + ) + + plan = _boundary("custream2").lower( + [composite], + _context(eef_by_arm={"left": "left_hand", "right": "right_hand"}), + ) + + groups = [segment for segment in plan.segments if isinstance(segment, ConcurrentGroupSegment)] + cartesian = [segment for segment in plan.segments if isinstance(segment, CartesianTrajectorySegment)] + assert len(groups) == 1 + assert {segment.eef_name for segment in cartesian} == {"left_hand", "right_hand"} + assert set(groups[0].member_segment_ids) == {segment.segment_id for segment in plan.segments[:-1]} + assert plan.metadata["schedulestream"]["has_concurrency"] is True + + +def test_symbol_provider_is_lazy_and_close_is_idempotent() -> None: + provider_calls = [] + closed = [] + boundary = ScheduleStreamCommandLowerer( + selector=_selector("custream"), + symbol_provider=lambda application: provider_calls.append(application) or _symbols(application), + owned_resource="world", + resource_closer=closed.append, + ) + assert provider_calls == [] + + boundary.lower([_Open(_World(), "arm")], _context()) + assert provider_calls == ["custream"] + boundary.close() + boundary.close() + assert closed == ["world"] + with pytest.raises(ScheduleStreamClosedError): + boundary.lower([_Open(_World(), "arm")], _context()) + + +@pytest.mark.parametrize( + ("commands", "error_type"), + [ + ([object()], UnsupportedScheduleStreamCommandError), + ([_Detach(_World(), arm="arm", parent="tool")], UnsupportedScheduleStreamCommandError), + ([_Trajectory(_World(), ("j1",), [[float("nan")]])], MalformedScheduleStreamCommandError), + ([_Open(_World(dt=0.1), "arm")], ScheduleStreamTimingError), + ], +) +def test_malformed_or_unsupported_commands_fail_closed(commands, error_type) -> None: + with pytest.raises(error_type): + _boundary("custream").lower(commands, _context()) + + +def test_exact_type_check_rejects_unknown_trajectory_subclass() -> None: + class FutureTrajectory(_Trajectory): + pass + + with pytest.raises(UnsupportedScheduleStreamCommandError): + _boundary("custream").lower( + [FutureTrajectory(_World(), ("j1",), [[0.0]])], + _context(), + ) + + +def test_composite_rejects_resource_conflicts_and_attachment_transitions() -> None: + world = _World() + conflicting = _Composite( + world, + [ + _V1LinkPath(world, "arm", "tool", [IDENTITY]), + _Open(world, "arm"), + ], + ) + attachment = _Composite( + world, + [_Attach(world, "cube", arm="arm", parent="tool"), _Open(world, "other")], + ) + + with pytest.raises(UnsupportedScheduleStreamCommandError, match="overlapping resources"): + _boundary("custream").lower([conflicting], _context()) + with pytest.raises(UnsupportedScheduleStreamCommandError, match="inside Composite"): + _boundary("custream").lower([attachment], _context()) diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_custream_v1.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_custream_v1.py new file mode 100644 index 0000000..6800b08 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_custream_v1.py @@ -0,0 +1,1445 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import numpy as np +import torch +from contextlib import nullcontext +from types import SimpleNamespace + +import pytest + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + DetachIntentSegment, + GoalPredicate, + JointTrajectorySegment, +) +from isaac_autodata_interfaces.autonomous.schedulestream import ( + MalformedScheduleStreamCommandError, + ScheduleStreamLoweringContext, + ScheduleStreamProviderError, + V1IsaacLabCommandPlanner, + V1IsaacLabPlannerConfig, + build_v1_isaaclab_world, + create_v1_isaaclab_command_planner, +) +from isaac_autodata_interfaces.autonomous.schedulestream.custream_v1 import ( + _action_body_offset, + _filter_v1_ik_joint_limits, + _scene_state_with_curobo_pose_order, + _validate_world_graspability, +) +from isaac_autodata_interfaces.autonomous.schedulestream.episode_planner import _world_eef_pose_reader +from isaac_autodata_interfaces.autonomous.schedulestream.goal_lowering import ScheduleStreamGoalSymbols + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +class _ActionManager: + active_terms = ("arm",) + + def get_term(self, name: str): + assert name == "arm" + offset = SimpleNamespace(pos=(0.0, 0.0, 0.1), rot=(0.0, 0.0, 0.0, 1.0)) + return SimpleNamespace(cfg=SimpleNamespace(body_name="tool", body_offset=offset)) + + +class _ActionManager3d: + active_terms = ("arm",) + + def get_term(self, name: str): + assert name == "arm" + offset = SimpleNamespace(pos=(0.1, 0.2, 0.3), rot=(0.0, 0.0, 0.0, 1.0)) + return SimpleNamespace(cfg=SimpleNamespace(body_name="tool", body_offset=offset)) + + +class _NativePlanner: + def __init__(self) -> None: + self.semantic_arm = "arm" + self.semantic_graspable_object = "cube" + self.base_env = SimpleNamespace(action_manager=_ActionManager()) + self.world = SimpleNamespace(time_step=0.05) + self.frames = [] + self.goal = "goal" + self.env = object() + + def current_link_matrix(self, link_name: str): + assert link_name == "tool" + return IDENTITY + + def plan_controller(self, env_id: int): + assert env_id == 0 + return SimpleNamespace( + joints=("j1", "j2"), + joint_positions=( + (0.0, 0.1), + (0.2, 0.3), + (0.2, 0.3), + (0.4, 0.5), + (0.4, 0.5), + ), + link_poses={ + "tool": ( + (0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0), + (0.2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0), + (0.2, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0), + (0.3, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0), + (0.3, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0), + ) + }, + gripper_actions=(1.0, 1.0, -1.0, -1.0, 1.0), + ) + + +class _PlannerToolNative(_NativePlanner): + def __init__(self) -> None: + super().__init__() + self.base_env = SimpleNamespace(action_manager=_ActionManager3d()) + self.world = SimpleNamespace( + get_arm_link=lambda arm: "ee_link", + time_step=0.05, + ) + + def current_link_matrix(self, link_name: str): + if link_name == "tool": + return IDENTITY + assert link_name == "ee_link" + return ( + (1.0, 0.0, 0.0, 0.1), + (0.0, 1.0, 0.0, 0.2), + (0.0, 0.0, 1.0, 0.3), + (0.0, 0.0, 0.0, 1.0), + ) + + +def _observed_pose(env_id: int, eef_name: str): + assert env_id == 0 + assert eef_name == "hand" + return ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.1), + (0.0, 0.0, 0.0, 1.0), + ) + + +def _rotated_observed_pose(env_id: int, eef_name: str): + assert env_id == 0 + assert eef_name == "hand" + return ( + (-1.0, 0.0, 0.0, 0.5), + (0.0, -1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.1), + (0.0, 0.0, 0.0, 1.0), + ) + + +def _planner_tool_observed_pose(env_id: int, eef_name: str): + assert env_id == 0 + assert eef_name == "hand" + return _PlannerToolNative().current_link_matrix("ee_link") + + +def _context(seed: int) -> ScheduleStreamLoweringContext: + return ScheduleStreamLoweringContext( + request_digest="request", + snapshot_digest="snapshot", + seed=seed, + goal=(GoalPredicate("on", "cube", "table"),), + eef_name="hand", + frame="world", + step_dt_s=0.05, + eef_by_link={"tool": "hand"}, + backend_version="test", + ) + + +def test_controller_dense_trace_preserves_native_body_offset_samples_and_attempt_seed() -> None: + seeds = [] + + def set_seed(*, seed: int) -> None: + seeds.append(seed) + + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=_observed_pose, + seed_setter=set_seed, + ) + + first = provider.plan_dense_trace(_context(11), link_name="tool") + second = provider.plan_dense_trace(_context(29), link_name="tool") + + assert seeds == [11, 29] + assert first is not None and second is not None + # Match upstream PathController: raw body z=0 plus the exactly configured and live-attested + # action offset z=0.1, with no inferred calibration transform. + assert first.poses[0][2][3] == pytest.approx(0.1) + assert first.poses[1][0][3] == pytest.approx(0.2) + assert first.poses[1][2][3] == pytest.approx(0.1) + assert first.joint_names == ("j1", "j2") + assert first.gripper_values == (1.0, 1.0, -1.0, -1.0, 1.0) + assert first.gripper_settle_steps == 4 + assert [(event.sample_index, event.operation, event.object_name) for event in first.attachment_events] == [ + (2, "attach", "cube"), + (4, "detach", "cube"), + ] + + +def test_controller_dense_trace_rejects_observed_frame_outside_configured_offset_attestation() -> None: + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=_rotated_observed_pose, + seed_setter=lambda *, seed: None, + ) + + with pytest.raises(ScheduleStreamProviderError, match="does not match the configured v1 action offset"): + provider.plan_dense_trace(_context(7), link_name="tool") + + +def test_controller_dense_trace_rejects_unconfigured_urdf_to_usd_frame_transform() -> None: + observed = ( + (-1.0, 0.0, 0.0, -0.13), + (0.0, 1.0, 0.0, -0.07), + (0.0, 0.0, -1.0, 0.06), + (0.0, 0.0, 0.0, 1.0), + ) + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=lambda env_id, eef_name: observed, + seed_setter=lambda *, seed: None, + ) + + with pytest.raises(ScheduleStreamProviderError, match="does not match the configured v1 action offset"): + provider.plan_dense_trace(_context(7), link_name="tool") + + +def test_controller_dense_trace_attests_configured_offset_on_every_attempt() -> None: + observations = [_observed_pose(0, "hand"), _observed_pose(0, "hand")] + second = [list(row) for row in observations[1]] + second[0][3] = 0.006 + observations[1] = tuple(tuple(row) for row in second) + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=lambda env_id, eef_name: observations.pop(0), + seed_setter=lambda *, seed: None, + ) + + assert provider.plan_dense_trace(_context(7), link_name="tool") is not None + with pytest.raises(ScheduleStreamProviderError, match="does not match the configured v1 action offset"): + provider.plan_dense_trace(_context(8), link_name="tool") + + +def test_controller_dense_trace_uses_attested_static_action_to_observed_eef_transform() -> None: + provider = V1IsaacLabCommandPlanner( + _PlannerToolNative(), + eef_pose_reader=_planner_tool_observed_pose, + seed_setter=lambda *, seed: None, + ) + + trace = provider.plan_dense_trace(_context(13), link_name="tool") + + assert trace is not None + assert tuple(trace.poses[0][index][3] for index in range(3)) == pytest.approx((0.1, 0.2, 0.3)) + assert tuple(trace.poses[1][index][3] for index in range(3)) == pytest.approx((0.3, 0.2, 0.3)) + assert trace.poses[2] == trace.poses[1] + assert tuple(trace.poses[3][index][3] for index in range(3)) == pytest.approx((0.4, 0.2, 0.3)) + assert trace.poses[4] == trace.poses[3] + assert provider._last_frame_evidence["planned_link_name"] == "tool" + attestation = provider._last_frame_evidence["static_frame_attestation"] + assert attestation["configured_offset_position_error_m"] == pytest.approx(0.0) + assert attestation["configured_offset_rotation_error_rad"] == pytest.approx(0.0) + + +def test_controller_dense_trace_records_observed_offset_without_using_it_as_calibration() -> None: + observed = [list(row) for row in _observed_pose(0, "hand")] + observed[0][3] = 0.001 + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=lambda env_id, eef_name: observed, + seed_setter=lambda *, seed: None, + ) + + trace = provider.plan_dense_trace(_context(17), link_name="tool") + + assert trace is not None + # Lowering remains anchored to the reviewed configured 0.1 m z offset, not the small live + # residual that is accepted only as attestation tolerance. + assert tuple(trace.poses[0][index][3] for index in range(3)) == pytest.approx((0.0, 0.0, 0.1)) + attestation = provider._last_frame_evidence["static_frame_attestation"] + assert tuple(attestation["action_to_observed_eef"][index][3] for index in range(3)) == pytest.approx( + (0.001, 0.0, 0.1) + ) + assert tuple(attestation["configured_action_offset"][index][3] for index in range(3)) == pytest.approx( + (0.0, 0.0, 0.1) + ) + assert attestation["configured_offset_position_error_m"] == pytest.approx(0.001) + + +def test_controller_task_plan_contains_only_current_ik_executor_segments() -> None: + provider = V1IsaacLabCommandPlanner( + _NativePlanner(), + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + ) + destination_placement = {"attested": True, "schema_version": 1} + grasp_geometry = {"attested": True, "schema_version": 1} + provider.native_planner.destination_placement_geometry = destination_placement + provider.native_planner.grasp_geometry = grasp_geometry + + plan = provider.plan_task_motion_plan(_context(3), link_name="tool") + + assert plan is not None + assert any(isinstance(segment, CartesianTrajectorySegment) for segment in plan.segments) + assert not any(isinstance(segment, JointTrajectorySegment) for segment in plan.segments) + cartesian = [segment for segment in plan.segments if isinstance(segment, CartesianTrajectorySegment)] + assert sum(segment.duration_s for segment in cartesian) == pytest.approx(5 * 0.05) + gripper = [segment for segment in plan.segments if segment.kind.value == "gripper_command"] + assert all(segment.settle_steps == 4 for segment in gripper) + attachments = [segment for segment in plan.segments if isinstance(segment, AttachIntentSegment)] + detachments = [segment for segment in plan.segments if isinstance(segment, DetachIntentSegment)] + assert [segment.object_name for segment in attachments] == ["cube"] + assert [segment.object_name for segment in detachments] == ["cube"] + assert plan.metadata["schedulestream"]["attachment_events_preserved"] is True + assert plan.metadata["schedulestream"]["destination_placement"] == destination_placement + assert plan.metadata["schedulestream"]["frame_evidence"]["destination_placement"] == destination_placement + assert plan.metadata["schedulestream"]["grasp_geometry"] == grasp_geometry + assert plan.metadata["schedulestream"]["frame_evidence"]["grasp_geometry"] == grasp_geometry + + +def test_controller_dense_trace_rejects_place_plan_without_release_transition() -> None: + native = _NativePlanner() + + def plan_controller(env_id: int): + controller = _NativePlanner().plan_controller(env_id) + controller.joint_positions = controller.joint_positions[:2] + controller.link_poses["tool"] = controller.link_poses["tool"][:2] + controller.gripper_actions = controller.gripper_actions[:2] + return controller + + native.plan_controller = plan_controller + provider = V1IsaacLabCommandPlanner( + native, + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + ) + + with pytest.raises(MalformedScheduleStreamCommandError, match="lifecycle does not match"): + provider.plan_dense_trace(_context(3), link_name="tool") + + +@pytest.mark.parametrize( + ("quaternion_xyzw", "expected_rotation"), + [ + ( + (0.0, 0.0, 0.0, 1.0), + ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)), + ), + ( + (0.0, 0.0, 2**-0.5, 2**-0.5), + ((0.0, -1.0, 0.0), (1.0, 0.0, 0.0), (0.0, 0.0, 1.0)), + ), + ], +) +def test_action_body_offset_converts_isaaclab_xyzw_to_curobo_wxyz( + quaternion_xyzw: tuple[float, float, float, float], + expected_rotation: tuple[tuple[float, float, float], ...], +) -> None: + offset = SimpleNamespace(pos=(0.1, 0.2, 0.3), rot=quaternion_xyzw) + term = SimpleNamespace(cfg=SimpleNamespace(body_name="tool", body_offset=offset)) + manager = SimpleNamespace(active_terms=("arm",), get_term=lambda name: term) + + matrix = _action_body_offset(SimpleNamespace(action_manager=manager), "tool") + + np.testing.assert_allclose(np.asarray(matrix)[:3, :3], expected_rotation, atol=1e-12) + np.testing.assert_allclose(np.asarray(matrix)[:3, 3], (0.1, 0.2, 0.3), atol=1e-12) + + +def test_world_reader_applies_nonzero_origin_without_changing_observed_rotation() -> None: + relative_eef = np.array(( + (0.0, -1.0, 0.0, 0.5), + (1.0, 0.0, 0.0, -0.25), + (0.0, 0.0, 1.0, 0.2), + (0.0, 0.0, 0.0, 1.0), + )) + adapter = SimpleNamespace(get_eef_poses=lambda env_ids: {"hand": np.stack((relative_eef,))}) + env = SimpleNamespace(scene=SimpleNamespace(env_origins=np.array(((2.0, 3.0, 4.0),)))) + observed = _world_eef_pose_reader(env, adapter)(0, "hand") + + np.testing.assert_allclose(np.asarray(observed)[:3, :3], relative_eef[:3, :3]) + np.testing.assert_allclose(np.asarray(observed)[:3, 3], (2.5, 2.75, 4.2)) + assert relative_eef[0, 3] == pytest.approx(0.5) + + +class _FakeWorld: + instances = [] + + def __init__(self, robot_config, objects, **kwargs) -> None: + self.robot_config = robot_config + self.objects = objects + self.kwargs = kwargs + self.joint_update = None + self.camera_pose = None + self.object_poses = {item.name: getattr(item, "pose", _WorldPose()) for item in objects} + self.__class__.instances.append(self) + + @property + def object_names(self) -> tuple[str, ...]: + return tuple(self.object_poses) + + def get_object_pose(self, name: str): + return self.object_poses[name] + + def get_object(self, name: str): + return next(item for item in self.objects if item.name == name) + + def set_object_pose(self, name: str, pose) -> None: + self.object_poses[name] = pose + + def set_joint_positions(self, names, positions) -> None: + self.joint_update = (tuple(names), tuple(positions)) + + def set_camera_pose(self, pose) -> None: + self.camera_pose = pose + + @property + def movable_names(self) -> tuple[str, ...]: + return tuple(item.name for item in self.objects if getattr(item, "grasp_config", None) is not None) + + +class _FakeGraspConfig: + def __init__(self, *, primitive: str, pitch_interval: str, generator=None) -> None: + self.primitive = primitive + self.pitch_interval = pitch_interval + self.generator = generator + + +class _FakeSurfaceConfig: + def __init__(self, *, xy_extend: float, z_offset: float) -> None: + self.xy_extend = xy_extend + self.z_offset = z_offset + + @property + def surface_extend(self): + return np.array((self.xy_extend, self.xy_extend, 0.0)) + + +class _WorldPose: + def __init__(self, matrix=None) -> None: + self.matrix = np.eye(4) if matrix is None else np.asarray(matrix, dtype=float) + + @classmethod + def from_pose7(cls, value): + x, y, z, qw, qx, qy, qz = (float(item) for item in value) + quaternion_norm = np.linalg.norm((qw, qx, qy, qz)) + qw, qx, qy, qz = (item / quaternion_norm for item in (qw, qx, qy, qz)) + matrix = np.array([ + [1.0 - 2.0 * (qy * qy + qz * qz), 2.0 * (qx * qy - qz * qw), 2.0 * (qx * qz + qy * qw), x], + [2.0 * (qx * qy + qz * qw), 1.0 - 2.0 * (qx * qx + qz * qz), 2.0 * (qy * qz - qx * qw), y], + [2.0 * (qx * qz - qy * qw), 2.0 * (qy * qz + qx * qw), 1.0 - 2.0 * (qx * qx + qy * qy), z], + [0.0, 0.0, 0.0, 1.0], + ]) + return cls(matrix) + + @classmethod + def translated(cls, x: float, y: float = 0.0, z: float = 0.0): + matrix = np.eye(4) + matrix[:3, 3] = (x, y, z) + return cls(matrix) + + def inverse(self): + return _WorldPose(np.linalg.inv(self.matrix)) + + def get_numpy_matrix(self): + return [self.matrix.copy()] + + +def _converted_object( + name: str, + *, + dimensions: tuple[float, float, float] | None = None, + center: tuple[float, float, float] = (0.0, 0.0, 0.0), + pose=None, + grasp_config=None, +): + if dimensions is None: + dimensions = (0.16, 0.16, 0.05) if name == "destination_bowl" else (0.06, 0.06, 0.06) + bounding_box = SimpleNamespace(dimensions=np.asarray(dimensions), pose=_WorldPose.translated(*center)) + return SimpleNamespace( + bounding_box=bounding_box, + grasp_config=grasp_config, + name=name, + pose=_WorldPose() if pose is None else pose, + surface_config=None, + ) + + +def _multiply_world_poses(*poses): + result = np.eye(4) + for pose in poses: + result = result @ pose.matrix + return _WorldPose(result) + + +def _fake_primitive_grasp_generator(primitive, dimensions, pitch_interval): + assert primitive == "cuboid" + assert len(dimensions) == 3 + assert pitch_interval == "top" + pitch = np.diag((1.0, -1.0, -1.0)) + for yaw in np.linspace(0.0, 2.0 * np.pi, num=4, endpoint=False): + cosine = np.cos(yaw) + sine = np.sin(yaw) + yaw_rotation = np.array(((cosine, -sine, 0.0), (sine, cosine, 0.0), (0.0, 0.0, 1.0))) + matrix = np.eye(4) + matrix[:3, :3] = pitch @ yaw_rotation + yield _WorldPose(matrix) + + +def _rigid_object(*, kinematic_enabled: bool | None = None, rigid_props_present: bool = True): + rigid_props = SimpleNamespace(kinematic_enabled=kinematic_enabled) if rigid_props_present else None + return SimpleNamespace(cfg=SimpleNamespace(spawn=SimpleNamespace(rigid_props=rigid_props))) + + +def _world_scene(*, rigid_objects: dict[str, object], basename: str = "panda_instanceable.usd"): + articulation = SimpleNamespace( + cfg=SimpleNamespace(spawn=SimpleNamespace(usd_path=f"omniverse://robots/{basename}")), + joint_names=("j1", "j2"), + ) + return SimpleNamespace( + articulations={"robot": articulation}, + env_origins=((0.0, 0.0, 0.0),), + rigid_objects=rigid_objects, + sim=SimpleNamespace(get_physics_dt=lambda: 0.01), + state={ + "articulation": { + "robot": { + "joint_position": ((0.1, 0.2),), + "root_pose": ((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),), + } + } + }, + ) + + +def _world_module(converted_objects): + return SimpleNamespace( + CAMERA_POSE="camera", + GraspConfig=_FakeGraspConfig, + SurfaceConfig=_FakeSurfaceConfig, + primitive_grasp_generator=_fake_primitive_grasp_generator, + World=_FakeWorld, + create_objects=lambda scene, env_id: converted_objects, + load_franka_config=lambda base_poses: {"base_poses": base_poses, "robot": "franka"}, + multiply_poses=_multiply_world_poses, + to_pose=_WorldPose.from_pose7, + ) + + +def _reviewed_profile_kwargs() -> dict[str, str]: + return { + "destination_asset_name": "bowl_ycb_robolab", + "destination_object": "destination_bowl", + "graspable_asset_name": "rubiks_cube_hot3d_robolab", + "graspable_object": "pick_cube", + } + + +def test_scene_state_pose_boundary_converts_xyzw_to_curobo_wxyz_without_mutation() -> None: + half_sqrt = 2**-0.5 + root_pose = torch.tensor([[0.4, -0.2, 0.1, 0.0, half_sqrt, 0.0, half_sqrt]], dtype=torch.float64) + state = { + "articulation": { + "robot": { + "joint_position": torch.tensor([[0.1, 0.2]], dtype=torch.float64), + "root_pose": root_pose, + } + } + } + + converted = _scene_state_with_curobo_pose_order(state, "robot") + + assert converted is not state + assert converted["articulation"]["robot"] is not state["articulation"]["robot"] + torch.testing.assert_close( + converted["articulation"]["robot"]["root_pose"], + torch.tensor([[0.4, -0.2, 0.1, half_sqrt, 0.0, half_sqrt, 0.0]], dtype=torch.float64), + ) + torch.testing.assert_close( + root_pose, + torch.tensor([[0.4, -0.2, 0.1, 0.0, half_sqrt, 0.0, half_sqrt]], dtype=torch.float64), + ) + + +@pytest.mark.parametrize("basename", ["panda_instanceable.usd", "franka_panda_hand_on_stand.usd"]) +def test_v1_world_factory_accepts_isaaclab_and_arena_franka_assets(basename: str) -> None: + _FakeWorld.instances.clear() + articulation = SimpleNamespace( + cfg=SimpleNamespace(spawn=SimpleNamespace(usd_path=f"omniverse://robots/{basename}")), + joint_names=("j1", "j2"), + ) + scene = SimpleNamespace( + articulations={"robot": articulation}, + env_origins=((0.0, 0.0, 0.0),), + rigid_objects={ + "destination_bowl": _rigid_object(kinematic_enabled=False), + "pick_cube": _rigid_object(rigid_props_present=False), + }, + sim=SimpleNamespace(get_physics_dt=lambda: 0.01), + state={ + "articulation": { + "robot": { + "joint_position": ((0.1, 0.2),), + "root_pose": ((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),), + } + } + }, + ) + module = SimpleNamespace( + CAMERA_POSE="camera", + GraspConfig=_FakeGraspConfig, + SurfaceConfig=_FakeSurfaceConfig, + primitive_grasp_generator=_fake_primitive_grasp_generator, + World=_FakeWorld, + create_objects=lambda scene, env_id: [ + _converted_object("pick_cube"), + _converted_object("destination_bowl"), + ], + load_franka_config=lambda base_poses: {"base_poses": base_poses, "robot": "franka"}, + multiply_poses=_multiply_world_poses, + to_pose=_WorldPose.from_pose7, + ) + + world = build_v1_isaaclab_world( + module, + scene, + V1IsaacLabPlannerConfig(scale_dt=5.0), + **_reviewed_profile_kwargs(), + ) + + assert world.kwargs["interpolation_dt"] == pytest.approx(0.05) + assert world.robot_config["base_poses"] is None + assert world.joint_update == (("j1", "j2"), (0.1, 0.2)) + assert world.camera_pose == "camera" + assert world.autodata_reference_frame_evidence == { + "converted_scene_frame": "enclosing_robot_prim", + "curobo_pose_quaternion_order": "wxyz", + "isaaclab_pose_quaternion_order": "xyzw", + "object_count": 2, + "planner_frame": "articulation_root", + "robot_prim_to_articulation_root": [list(row) for row in np.eye(4)], + } + + +def test_v1_world_factory_rebases_robot_prim_obstacles_into_rotated_articulation_root() -> None: + half_sqrt = 2**-0.5 + root_relative = _WorldPose.from_pose7((0.4, -0.2, 0.1, half_sqrt, 0.0, 0.0, half_sqrt)) + object_robot_relative = _WorldPose.from_pose7((0.8, 0.1, 0.2, 1.0, 0.0, 0.0, 0.0)) + converted = _converted_object("pick_cube", pose=object_robot_relative) + destination = _converted_object("destination_bowl") + scene = _world_scene( + rigid_objects={ + "destination_bowl": _rigid_object(kinematic_enabled=False), + "pick_cube": _rigid_object(rigid_props_present=False), + } + ) + scene.env_origins = ((2.0, 3.0, 0.5),) + scene.state["articulation"]["robot"]["root_pose"] = ((2.4, 2.8, 0.6, 0.0, 0.0, half_sqrt, half_sqrt),) + + world = build_v1_isaaclab_world( + _world_module([converted, destination]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + expected = np.linalg.inv(root_relative.matrix) @ object_robot_relative.matrix + np.testing.assert_allclose(world.object_poses["pick_cube"].matrix, expected, atol=1e-12) + np.testing.assert_allclose( + world.autodata_reference_frame_evidence["robot_prim_to_articulation_root"], + root_relative.matrix, + atol=1e-12, + ) + + +def test_v1_world_factory_rejects_missing_environment_origin() -> None: + converted = _converted_object("pick_cube") + destination = _converted_object("destination_bowl") + scene = _world_scene( + rigid_objects={ + "destination_bowl": _rigid_object(kinematic_enabled=False), + "pick_cube": _rigid_object(rigid_props_present=False), + } + ) + del scene.env_origins + + with pytest.raises(ScheduleStreamProviderError, match="scene.env_origins is required"): + build_v1_isaaclab_world( + _world_module([converted, destination]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + +def test_v1_world_factory_makes_only_task_selected_dynamic_object_graspable() -> None: + pick_cube = _converted_object("pick_cube", center=(0.01, -0.02, 0.003)) + destination_bowl = _converted_object("destination_bowl", center=(-0.004, 0.005, 0.006)) + kinematic_marker = SimpleNamespace(name="kinematic_marker", grasp_config="stale") + static_mesh = SimpleNamespace(name="/World/floor", grasp_config=None) + scene = _world_scene( + rigid_objects={ + # Arena commonly leaves rigid_props unset for ordinary dynamic RigidObjects. + "pick_cube": _rigid_object(rigid_props_present=False), + "destination_bowl": _rigid_object(kinematic_enabled=False), + "kinematic_marker": _rigid_object(kinematic_enabled=True), + } + ) + + world = build_v1_isaaclab_world( + _world_module([pick_cube, destination_bowl, kinematic_marker, static_mesh]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + assert world.objects == [pick_cube, destination_bowl, kinematic_marker, static_mesh] + assert isinstance(pick_cube.grasp_config, _FakeGraspConfig) + assert pick_cube.grasp_config.primitive == "cuboid" + assert pick_cube.grasp_config.pitch_interval == "top" + assert isinstance(pick_cube.grasp_config.generator, tuple) + assert len(pick_cube.grasp_config.generator) == 4 + assert destination_bowl.grasp_config is None + assert kinematic_marker.grasp_config is None + assert static_mesh.grasp_config is None + assert world.movable_names == ("pick_cube",) + assert destination_bowl.surface_config.xy_extend == pytest.approx(-0.16) + assert destination_bowl.surface_config.z_offset == pytest.approx(-0.025) + placement_geometry = world.autodata_destination_placement_geometry + assert placement_geometry["general_inside_semantics"] is False + assert placement_geometry["sampled_surface_extent_m"] == pytest.approx([0.0, 0.0, 0.0]) + assert placement_geometry["predicted_aabb_center_offset_m"] == pytest.approx([0.0, 0.0, 0.03]) + assert placement_geometry["subject"]["aabb_center_in_converted_object_origin_m"] == pytest.approx( + [0.01, -0.02, 0.003] + ) + assert placement_geometry["destination"]["aabb_center_in_converted_object_origin_m"] == pytest.approx( + [-0.004, 0.005, 0.006] + ) + grasp_geometry = world.autodata_grasp_geometry + assert grasp_geometry["grasp_count"] == 4 + assert grasp_geometry["generator_storage"] == "reusable_finite_tuple" + assert len(grasp_geometry["primitive_link_from_aabb_center_transforms"]) == 4 + assert len(grasp_geometry["link_from_object_transforms"]) == 4 + + +def test_v1_world_factory_derives_exact_center_profile_from_attested_live_aabbs() -> None: + source_dimensions = (0.058285847306251526, 0.057770855724811554, 0.05796363018453121) + destination_dimensions = (0.158, 0.151, 0.054) + pick_cube = _converted_object("pick_cube", dimensions=source_dimensions) + destination_bowl = _converted_object("destination_bowl", dimensions=destination_dimensions) + scene = _world_scene( + rigid_objects={ + "pick_cube": _rigid_object(rigid_props_present=False), + "destination_bowl": _rigid_object(kinematic_enabled=False), + } + ) + + world = build_v1_isaaclab_world( + _world_module([pick_cube, destination_bowl]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + expected_z_offset = 0.03 - 0.5 * (destination_dimensions[2] + source_dimensions[2]) + assert destination_bowl.surface_config.xy_extend == pytest.approx(-max(destination_dimensions[:2])) + assert destination_bowl.surface_config.z_offset == pytest.approx(expected_z_offset) + placement_geometry = world.autodata_destination_placement_geometry + assert placement_geometry["sampled_surface_extent_m"] == pytest.approx([0.0, 0.0, 0.0]) + assert placement_geometry["predicted_aabb_center_offset_m"] == pytest.approx([0.0, 0.0, 0.03]) + assert placement_geometry["subject"]["aabb_dimensions_m"] == pytest.approx(source_dimensions) + assert placement_geometry["subject"]["aabb_dimension_bounds_m"] == [[0.05, 0.065]] * 3 + assert placement_geometry["destination"]["aabb_dimensions_m"] == pytest.approx(destination_dimensions) + assert placement_geometry["destination"]["aabb_dimension_bounds_m"] == [ + [0.14, 0.17], + [0.14, 0.17], + [0.04, 0.07], + ] + assert placement_geometry["surface_config"]["derivation"] == { + "desired_aabb_center_vertical_offset_m": 0.03, + "inputs": "attested_converted_local_aabb_dimensions_m", + "xy_extend_formula": "-max(destination_aabb_width_m,destination_aabb_depth_m)", + "z_offset_formula": "desired_center_z_m-0.5*(destination_aabb_height_m+subject_aabb_height_m)", + } + + +def test_v1_analytical_grasps_preserve_world_aabb_center_with_noncommuting_pose() -> None: + def pose_z(theta: float, translation: tuple[float, float, float]) -> _WorldPose: + cosine = np.cos(theta) + sine = np.sin(theta) + matrix = np.array([ + [cosine, -sine, 0.0, translation[0]], + [sine, cosine, 0.0, translation[1]], + [0.0, 0.0, 1.0, translation[2]], + [0.0, 0.0, 0.0, 1.0], + ]) + return _WorldPose(matrix) + + object_pose = pose_z(-0.41, (0.52, -0.13, 0.035)) + bounding_box_pose = pose_z(0.37, (-0.0101597, 0.0289287, -0.002404)) + pick_cube = _converted_object( + "pick_cube", + dimensions=(0.0582858473, 0.0577708557, 0.0579636302), + pose=object_pose, + ) + pick_cube.bounding_box.pose = bounding_box_pose + destination_bowl = _converted_object("destination_bowl") + scene = _world_scene( + rigid_objects={ + "pick_cube": _rigid_object(rigid_props_present=False), + "destination_bowl": _rigid_object(kinematic_enabled=False), + } + ) + + world = build_v1_isaaclab_world( + _world_module([pick_cube, destination_bowl]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + primitive_matrices = [ + np.asarray(value) for value in world.autodata_grasp_geometry["primitive_link_from_aabb_center_transforms"] + ] + link_from_object_matrices = [pose.matrix for pose in pick_cube.grasp_config.generator] + world_from_aabb = object_pose.matrix @ bounding_box_pose.matrix + buggy_center_errors = [] + for primitive, link_from_object in zip(primitive_matrices, link_from_object_matrices): + world_from_link = object_pose.matrix @ np.linalg.inv(link_from_object) + expected_world_from_link = world_from_aabb @ np.linalg.inv(primitive) + np.testing.assert_allclose(world_from_link, expected_world_from_link, atol=1e-12) + np.testing.assert_allclose(world_from_link[:3, 3], world_from_aabb[:3, 3], atol=1e-12) + buggy_world_from_link = object_pose.matrix @ np.linalg.inv(primitive) @ bounding_box_pose.matrix + buggy_center_errors.append(np.linalg.norm(buggy_world_from_link[:3, 3] - world_from_aabb[:3, 3])) + assert max(buggy_center_errors) > 0.02 + + +@pytest.mark.parametrize("failure", ["count", "matrix", "noncallable"]) +def test_v1_world_factory_rejects_malformed_analytical_grasp_sources(failure: str) -> None: + pick_cube = _converted_object("pick_cube") + destination_bowl = _converted_object("destination_bowl") + scene = _world_scene( + rigid_objects={ + "pick_cube": _rigid_object(rigid_props_present=False), + "destination_bowl": _rigid_object(kinematic_enabled=False), + } + ) + if failure == "count": + + def generator(*args): + del args + return iter((_WorldPose(),) * 3) + + message = "exactly four poses" + elif failure == "matrix": + malformed = _WorldPose() + malformed.matrix[0, 0] = np.nan + + def generator(*args): + del args + return iter((_WorldPose(), _WorldPose.translated(0.0), malformed, _WorldPose())) + + message = "finite rigid pose" + else: + generator = object() + message = "not callable" + + with pytest.raises(ScheduleStreamProviderError, match=message): + build_v1_isaaclab_world( + _world_module([pick_cube, destination_bowl]), + scene, + V1IsaacLabPlannerConfig(), + primitive_grasp_generator=generator, + **_reviewed_profile_kwargs(), + ) + + +def test_world_graspability_rejects_missing_or_tampered_grasp_geometry() -> None: + with pytest.raises(ScheduleStreamProviderError, match="grasp geometry"): + _validate_world_graspability(SimpleNamespace(movable_names=("cube",)), "cube") + + grasp_geometry = _factory_grasp_geometry("cube") + grasp_geometry["link_from_object_transforms"][0][0][3] = 1e-6 + world = SimpleNamespace( + autodata_grasp_geometry=grasp_geometry, + movable_names=("cube",), + ) + with pytest.raises(ScheduleStreamProviderError, match="composition formula"): + _validate_world_graspability(world, "cube") + + +@pytest.mark.parametrize( + ("source_dimensions", "destination_dimensions"), + [ + ((0.049, 0.058, 0.058), (0.16, 0.16, 0.05)), + ((0.058, 0.058, 0.058), (0.16, 0.171, 0.05)), + ], +) +def test_v1_world_factory_rejects_geometry_outside_name_pinned_bounds( + source_dimensions: tuple[float, float, float], + destination_dimensions: tuple[float, float, float], +) -> None: + scene = _world_scene( + rigid_objects={ + "pick_cube": _rigid_object(rigid_props_present=False), + "destination_bowl": _rigid_object(kinematic_enabled=False), + } + ) + converted = [ + _converted_object("pick_cube", dimensions=source_dimensions), + _converted_object("destination_bowl", dimensions=destination_dimensions), + ] + + with pytest.raises(ScheduleStreamProviderError, match="outside the reviewed per-axis bounds"): + build_v1_isaaclab_world( + _world_module(converted), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + +@pytest.mark.parametrize( + ("converted_objects", "message"), + [ + ([SimpleNamespace(name="not_cube", grasp_config=None)], "did not bind"), + ( + [ + SimpleNamespace(name="pick_cube", grasp_config=None), + SimpleNamespace(name="pick_cube", grasp_config=None), + ], + "bound ambiguously", + ), + ], +) +def test_v1_world_factory_fails_closed_on_missing_or_ambiguous_rigid_binding( + converted_objects, + message: str, +) -> None: + scene = _world_scene(rigid_objects={"pick_cube": _rigid_object(rigid_props_present=False)}) + + with pytest.raises(ScheduleStreamProviderError, match=message): + build_v1_isaaclab_world( + _world_module(converted_objects), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + +def test_v1_world_factory_rejects_missing_or_kinematic_task_selected_object() -> None: + converted = SimpleNamespace(name="destination_bowl", grasp_config=None) + scene = _world_scene(rigid_objects={"destination_bowl": _rigid_object(kinematic_enabled=False)}) + + with pytest.raises(ScheduleStreamProviderError, match="absent"): + build_v1_isaaclab_world( + _world_module([converted]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + scene = _world_scene(rigid_objects={"pick_cube": _rigid_object(kinematic_enabled=True)}) + with pytest.raises(ScheduleStreamProviderError, match="explicitly kinematic"): + build_v1_isaaclab_world( + _world_module([SimpleNamespace(name="pick_cube", grasp_config=None)]), + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + +def test_v1_world_factory_rejects_unreviewed_robot_asset() -> None: + articulation = SimpleNamespace( + cfg=SimpleNamespace(spawn=SimpleNamespace(usd_path="omniverse://robots/unknown.usd")), + joint_names=("j1",), + ) + scene = SimpleNamespace( + articulations={"robot": articulation}, + rigid_objects={}, + sim=SimpleNamespace(get_physics_dt=lambda: 0.01), + ) + module = SimpleNamespace(create_objects=lambda scene, env_id: [], load_franka_config=lambda base_poses: {}) + + with pytest.raises(ScheduleStreamProviderError, match="unsupported v1 robot USD"): + build_v1_isaaclab_world( + module, + scene, + V1IsaacLabPlannerConfig(), + **_reviewed_profile_kwargs(), + ) + + +class _Clause: + def __init__(self, values) -> None: + self.values = tuple(values) + + def __and__(self, other): + return _Clause(self.values + other.values) + + +class _BasePlanner: + @property + def scene(self): + return self.env.scene + + def pose(self, name: str, state=None): + return getattr(self.env, "root_poses", {}).get(name, _FactoryPose()) + + def set_env_state(self, env_id: int, state=None, **kwargs) -> None: + self.last_env_id = env_id + for name, root_pose in getattr(self.env, "root_poses", {}).items(): + self.world.set_object_pose(name, root_pose) + + +class _FactoryPose: + def __init__(self, matrix=None) -> None: + self.matrix = np.eye(4) if matrix is None else np.asarray(matrix, dtype=float) + + @classmethod + def translated(cls, x: float, y: float = 0.0, z: float = 0.0): + matrix = np.eye(4) + matrix[:3, 3] = (x, y, z) + return cls(matrix) + + def inverse(self): + return _FactoryPose(np.linalg.inv(self.matrix)) + + def get_numpy_matrix(self): + return [self.matrix.copy()] + + +def _multiply_factory_poses(*poses): + result = np.eye(4) + for pose in poses: + result = result @ pose.matrix + return _FactoryPose(result) + + +class _FactoryWorld: + arms = ("arm",) + movable_names = ("cube",) + + def __init__(self, object_poses=None) -> None: + self.object_poses = object_poses or {"cube": _FactoryPose.translated(0.03, -0.01, 0.002)} + self.object_pose_updates = [] + grasp_poses = tuple( + _FactoryPose(pose.matrix) for pose in _fake_primitive_grasp_generator("cuboid", (0.06, 0.06, 0.06), "top") + ) + self.objects = { + name: SimpleNamespace( + grasp_config=( + _FakeGraspConfig(primitive="cuboid", pitch_interval="top", generator=grasp_poses) + if name == "cube" + else None + ) + ) + for name in self.object_poses + } + + def set_retract_conf(self) -> None: + self.retract_set = True + + def initialize(self, batch_size: int) -> None: + self.batch_size = batch_size + + def link_iterative_inverse_kinematics(self, *args, **kwargs): + raise AssertionError("test world did not expect live IK") + + def configuration(self): + return "configuration" + + def get_object_pose(self, name: str): + return self.object_poses[name] + + def get_object(self, name: str): + return self.objects[name] + + def set_object_pose(self, name: str, pose) -> None: + self.object_poses[name] = pose + self.object_pose_updates.append((name, pose)) + + +def _factory_grasp_geometry(subject: str) -> dict: + primitive = [pose.matrix.tolist() for pose in _fake_primitive_grasp_generator("cuboid", (0.06,) * 3, "top")] + link_from_object = [[list(row) for row in transform] for transform in primitive] + return { + "asset_name": "rubiks_cube_hot3d_robolab", + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "converted_object_origin_from_aabb_center": np.eye(4).tolist(), + "link_from_object_transforms": link_from_object, + "generator_storage": "reusable_finite_tuple", + "grasp_count": 4, + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": subject, + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "primitive_link_from_aabb_center_transforms": primitive, + "profile": "franka_rubiks_cube_offcenter_cuboid_top_v1", + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + + +def _factory_destination_placement_geometry(subject: str, destination: str) -> dict: + identity = [list(row) for row in np.eye(4)] + return { + "attested": True, + "destination": { + "asset_name": "bowl_ycb_robolab", + "object_id": destination, + "aabb_center_in_converted_object_origin_m": [0.0, 0.0, 0.0], + "aabb_dimensions_m": [0.16, 0.16, 0.05], + "aabb_kind": "converted_mesh_local_axis_aligned_bounding_box", + "converted_object_origin_from_aabb_center": identity, + }, + "frame_convention": "parent_from_child_homogeneous_4x4", + "general_inside_semantics": False, + "limitations": ["test_surrogate"], + "placement_model": "destination_local_aabb_top_plane_shifted_downward", + "predicted_aabb_center_offset_m": [0.0, 0.0, 0.03], + "profile": "franka_rubiks_cube_to_ycb_bowl_aabb_top_plane_v1", + "relation": "on", + "sampled_surface_extent_m": [0.0, 0.0, 0.0], + "schema_version": 1, + "subject": { + "asset_name": "rubiks_cube_hot3d_robolab", + "object_id": subject, + "aabb_center_in_converted_object_origin_m": [0.0, 0.0, 0.0], + "aabb_dimensions_m": [0.06, 0.06, 0.06], + "aabb_kind": "converted_mesh_local_axis_aligned_bounding_box", + "converted_object_origin_from_aabb_center": identity, + }, + "surface_config": { + "implementation": "schedulestream.applications.custream.object.SurfaceConfig", + "xy_extend_m": -0.16, + "z_offset_m": -0.025, + }, + "vertical_evidence_corridor_m": 0.04, + } + + +def test_v1_ik_filter_rejects_violations_and_submilliradian_limit_candidates() -> None: + positions = torch.tensor([[ + [[0.0, 0.2], [0.1, 0.3]], + [[1.02, 0.0], [0.0, 0.0]], + [[0.9995, 0.0], [0.0, 0.0]], + ]]) + joint_state = SimpleNamespace(position=positions) + distances = torch.tensor([[0.1, 0.2, 0.3]]) + + def get_limit_distances(state): + lower_difference = -1.0 - state.position + upper_difference = state.position - 1.0 + return torch.maximum(lower_difference, upper_difference) + + world = SimpleNamespace( + autodata_ik_joint_limit_evidence={ + "accepted_candidates": 0, + "calls": 0, + "candidate_solutions": 0, + "joint_limit_margin_rad": 1e-3, + "policy": "all_waypoints_inside_native_position_limits", + "rejected_candidates": 0, + }, + get_limit_distances=get_limit_distances, + ) + + returned_state, filtered = _filter_v1_ik_joint_limits(world, joint_state, distances) + + assert returned_state is joint_state + assert filtered[0, 0].item() == pytest.approx(0.1) + assert torch.isinf(filtered[0, 1:]).all() + assert world.autodata_ik_joint_limit_evidence == { + "accepted_candidates": 1, + "calls": 1, + "candidate_solutions": 3, + "joint_limit_margin_rad": 1e-3, + "minimum_observed_margin_rad": pytest.approx(-0.02), + "policy": "all_waypoints_inside_native_position_limits", + "rejected_candidates": 2, + } + + +def test_v1_ik_filter_normalizes_every_nonfinite_distance_sentinel() -> None: + positions = torch.zeros((1, 4, 1, 1), dtype=torch.float32) + joint_state = SimpleNamespace(position=positions) + distances = torch.tensor([[0.1, torch.nan, -torch.inf, torch.inf]], dtype=torch.float32) + world = SimpleNamespace( + autodata_ik_joint_limit_evidence={ + "accepted_candidates": 0, + "calls": 0, + "candidate_solutions": 0, + "joint_limit_margin_rad": 1e-3, + "policy": "all_waypoints_inside_native_position_limits", + "rejected_candidates": 0, + }, + get_limit_distances=lambda state: torch.full_like(state.position, -1.0), + ) + + _, filtered = _filter_v1_ik_joint_limits(world, joint_state, distances) + + assert filtered[0, 0].item() == pytest.approx(0.1) + assert torch.equal(filtered[0, 1:], torch.full((3,), torch.inf)) + assert world.autodata_ik_joint_limit_evidence["candidate_solutions"] == 1 + assert world.autodata_ik_joint_limit_evidence["accepted_candidates"] == 1 + + +@pytest.mark.parametrize("dtype", [torch.int64, torch.float16, torch.bfloat16, torch.complex64]) +def test_v1_ik_filter_rejects_unsupported_distance_dtype(dtype: torch.dtype) -> None: + positions = torch.zeros((1, 1, 1, 1), dtype=torch.float32) + joint_state = SimpleNamespace(position=positions) + distances = torch.zeros((1, 1), dtype=dtype) + world = SimpleNamespace(get_limit_distances=lambda state: torch.full_like(state.position, -1.0)) + + with pytest.raises(ScheduleStreamProviderError, match="distances must use float32 or float64"): + _filter_v1_ik_joint_limits(world, joint_state, distances) + + +def test_concrete_v1_factory_compiles_semantic_goal_with_injected_runtime() -> None: + fake_module = SimpleNamespace( + CAMERA_POSE="camera", + Commands=SimpleNamespace(flatten=lambda commands: iter(commands)), + GraspConfig=_FakeGraspConfig, + Planner=_BasePlanner, + World=object, + animate_commands=lambda *args, **kwargs: [], + create_controller=lambda *args, **kwargs: None, + create_objects=lambda *args, **kwargs: [], + load_franka_config=lambda *args, **kwargs: {}, + multiply_poses=_multiply_factory_poses, + solve_tamp=lambda *args, **kwargs: None, + timeout_context=lambda **kwargs: nullcontext(), + ) + world = _FactoryWorld({ + "cube": _FactoryPose.translated(0.03, -0.01, 0.002), + "table": _FactoryPose(), + }) + world.autodata_destination_placement_geometry = _factory_destination_placement_geometry("cube", "table") + world.autodata_grasp_geometry = _factory_grasp_geometry("cube") + symbols = ScheduleStreamGoalSymbols( + attached_equals=lambda subject, target: _Clause((("attached", subject, target),)), + holding_equals=lambda arm, subject: _Clause((("holding", arm, subject),)), + ) + env = SimpleNamespace( + root_poses={"cube": _FactoryPose(), "table": _FactoryPose()}, + scene=SimpleNamespace( + rigid_objects={ + "cube": _rigid_object(rigid_props_present=False), + "table": _rigid_object(kinematic_enabled=False), + } + ), + ) + + provider = create_v1_isaaclab_command_planner( + env, + (GoalPredicate("on", "cube", "table"),), + graspable_object="cube", + destination_object="table", + graspable_asset_name="rubiks_cube_hot3d_robolab", + destination_asset_name="bowl_ycb_robolab", + module_loader=lambda: fake_module, + goal_symbols_loader=lambda application: symbols, + world_factory=lambda module, scene, config, **kwargs: world, + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + ) + + assert provider.arm == "arm" + assert provider.native_planner.goal.values == (("attached", "cube", "table"),) + assert world.batch_size == 10 + assert provider.native_planner.grasp_configuration["task_selected_graspable_object"] == "cube" + assert provider.native_planner.grasp_configuration["world_movable_names"] == ["cube"] + assert provider.native_planner.grasp_configuration["grasp_geometry"]["grasp_count"] == 4 + assert provider.native_planner.grasp_geometry["profile"] == "franka_rubiks_cube_offcenter_cuboid_top_v1" + np.testing.assert_allclose( + provider.native_planner.object_pose_offset_evidence["cube"], + _FactoryPose.translated(0.03, -0.01, 0.002).matrix, + ) + assert provider.native_planner.destination_placement_geometry["subject"][ + "aabb_center_in_isaac_rigid_root_m" + ] == pytest.approx([0.03, -0.01, 0.002]) + + env.root_poses["cube"] = _FactoryPose.translated(0.2, 0.1, 0.0) + provider.native_planner.set_env_state(0) + + np.testing.assert_allclose( + world.object_poses["cube"].matrix, + _FactoryPose.translated(0.23, 0.09, 0.002).matrix, + ) + + +def test_root_to_mesh_restore_preserves_rotated_noncommuting_offsets_after_upstream_stomp() -> None: + def pose_z(theta: float, translation: tuple[float, float, float]) -> _FactoryPose: + cosine = np.cos(theta) + sine = np.sin(theta) + matrix = np.array([ + [cosine, -sine, 0.0, translation[0]], + [sine, cosine, 0.0, translation[1]], + [0.0, 0.0, 1.0, translation[2]], + [0.0, 0.0, 0.0, 1.0], + ]) + return _FactoryPose(matrix) + + fake_module = SimpleNamespace( + CAMERA_POSE="camera", + Commands=SimpleNamespace(flatten=lambda commands: iter(commands)), + GraspConfig=_FakeGraspConfig, + Planner=_BasePlanner, + World=object, + animate_commands=lambda *args, **kwargs: [], + create_controller=lambda *args, **kwargs: None, + create_objects=lambda *args, **kwargs: [], + load_franka_config=lambda *args, **kwargs: {}, + multiply_poses=_multiply_factory_poses, + solve_tamp=lambda *args, **kwargs: None, + timeout_context=lambda **kwargs: nullcontext(), + ) + initial_roots = { + "cube": pose_z(np.pi / 2.0, (0.2, -0.1, 0.0)), + "bowl": pose_z(-np.pi / 4.0, (-0.3, 0.25, 0.01)), + } + offsets = { + "cube": pose_z(np.pi / 3.0, (0.03, -0.02, 0.005)), + "bowl": pose_z(-np.pi / 6.0, (-0.04, 0.01, 0.02)), + } + world = _FactoryWorld({name: _multiply_factory_poses(initial_roots[name], offsets[name]) for name in initial_roots}) + world.autodata_destination_placement_geometry = _factory_destination_placement_geometry("cube", "bowl") + world.autodata_grasp_geometry = _factory_grasp_geometry("cube") + symbols = ScheduleStreamGoalSymbols( + attached_equals=lambda subject, target: _Clause((("attached", subject, target),)), + holding_equals=lambda arm, subject: _Clause((("holding", arm, subject),)), + ) + env = SimpleNamespace( + root_poses=initial_roots, + scene=SimpleNamespace( + rigid_objects={ + "cube": _rigid_object(rigid_props_present=False), + "bowl": _rigid_object(rigid_props_present=False), + } + ), + ) + provider = create_v1_isaaclab_command_planner( + env, + (GoalPredicate("on", "cube", "bowl"),), + graspable_object="cube", + destination_object="bowl", + graspable_asset_name="rubiks_cube_hot3d_robolab", + destination_asset_name="bowl_ycb_robolab", + module_loader=lambda: fake_module, + goal_symbols_loader=lambda application: symbols, + world_factory=lambda module, scene, config, **kwargs: world, + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + ) + new_roots = { + "cube": pose_z(-np.pi / 5.0, (0.45, 0.15, 0.03)), + "bowl": pose_z(np.pi / 7.0, (-0.1, -0.2, 0.04)), + } + env.root_poses = new_roots + + provider.native_planner.set_env_state(0) + + for name in ("cube", "bowl"): + expected = _multiply_factory_poses(new_roots[name], offsets[name]).matrix + np.testing.assert_allclose(world.object_poses[name].matrix, expected, atol=1e-12) + assert not np.allclose(world.object_poses[name].matrix, new_roots[name].matrix) + + +def test_concrete_v1_factory_rejects_explicitly_kinematic_goal_subject() -> None: + fake_module = SimpleNamespace( + CAMERA_POSE="camera", + Commands=SimpleNamespace(flatten=lambda commands: iter(commands)), + GraspConfig=_FakeGraspConfig, + Planner=_BasePlanner, + World=object, + animate_commands=lambda *args, **kwargs: [], + create_controller=lambda *args, **kwargs: None, + create_objects=lambda *args, **kwargs: [], + load_franka_config=lambda *args, **kwargs: {}, + multiply_poses=_multiply_factory_poses, + solve_tamp=lambda *args, **kwargs: None, + timeout_context=lambda **kwargs: nullcontext(), + ) + env = SimpleNamespace(scene=SimpleNamespace(rigid_objects={"cube": _rigid_object(kinematic_enabled=True)})) + world_factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match="explicitly kinematic"): + create_v1_isaaclab_command_planner( + env, + (GoalPredicate("on", "cube", "table"),), + graspable_object="cube", + destination_object="table", + graspable_asset_name="rubiks_cube_hot3d_robolab", + destination_asset_name="bowl_ycb_robolab", + module_loader=lambda: fake_module, + world_factory=lambda module, scene, config, **kwargs: world_factory_calls.append(scene), + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + ) + + assert world_factory_calls == [] + + +def test_concrete_v1_factory_closes_staged_world_when_initialization_fails() -> None: + fake_module = SimpleNamespace( + CAMERA_POSE="camera", + Commands=SimpleNamespace(flatten=lambda commands: iter(commands)), + GraspConfig=_FakeGraspConfig, + Planner=_BasePlanner, + World=object, + animate_commands=lambda *args, **kwargs: [], + create_controller=lambda *args, **kwargs: None, + create_objects=lambda *args, **kwargs: [], + load_franka_config=lambda *args, **kwargs: {}, + multiply_poses=_multiply_factory_poses, + solve_tamp=lambda *args, **kwargs: None, + timeout_context=lambda **kwargs: nullcontext(), + ) + + class FailingWorld(_FactoryWorld): + def initialize(self, batch_size: int) -> None: + raise RuntimeError("GPU initialization failed") + + world = FailingWorld() + closed = [] + env = SimpleNamespace(scene=SimpleNamespace(rigid_objects={"cube": _rigid_object(rigid_props_present=False)})) + + with pytest.raises(ScheduleStreamProviderError, match="failed to construct semantic v1 planner"): + create_v1_isaaclab_command_planner( + env, + (GoalPredicate("on", "cube", "table"),), + graspable_object="cube", + destination_object="table", + graspable_asset_name="rubiks_cube_hot3d_robolab", + destination_asset_name="bowl_ycb_robolab", + module_loader=lambda: fake_module, + world_factory=lambda module, scene, config, **kwargs: world, + eef_pose_reader=_observed_pose, + seed_setter=lambda *, seed: None, + world_closer=closed.append, + ) + + assert closed == [world] diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_episode_planner.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_episode_planner.py new file mode 100644 index 0000000..a884172 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_episode_planner.py @@ -0,0 +1,278 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +from types import SimpleNamespace + +import pytest + +from isaac_autodata_interfaces.autonomous.schedulestream import ScheduleStreamProviderError +from isaac_autodata_interfaces.autonomous.schedulestream.episode_planner import create_schedulestream_episode_planner + +_PINNED_PANDA_USD = ( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1/Isaac/IsaacLab/" + "Robots/FrankaEmika/panda_instanceable.usd" +) +_PINNED_PANDA_USD_BYTES = 8_038 +_PINNED_PANDA_USD_SHA256 = "7f5a0c0aa6760cfbd348e08bc464d4b94341f027f51c2d9e42406ceefcc7787f" +_REGISTRY_PANDA_USD = ( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1/Isaac/IsaacLab/" + "Arena/assets/robot_library/franka_panda_hand_on_stand.usd" +) + + +class _ActionManager: + active_terms = ("arm",) + + def get_term(self, name: str): + assert name == "arm" + return SimpleNamespace(cfg=SimpleNamespace(body_name="panda_hand")) + + +def _request( + *, + task_pick: str = "pick_cube", + task_destination: str = "destination_bowl", + goal_subject: str = "pick_cube", + goal_destination: str = "destination_bowl", +): + planner = SimpleNamespace( + animate=False, + batch_size=8, + collisions=True, + interpolation_dt_s=0.02, + max_time_s=10.0, + profile=False, + ) + planner.to_dict = lambda: {"batch_size": 8, "collisions": True} + return SimpleNamespace( + generation=SimpleNamespace(num_envs=1), + goal_stages=( + SimpleNamespace( + spatial_constraints=(SimpleNamespace(kind="on", subject=goal_subject, reference=goal_destination),) + ), + ), + graph_digest="a" * 64, + linked_graph={ + "nodes": [ + { + "id": "pick_cube", + "name": "rubiks_cube_hot3d_robolab", + "params": {}, + "type": "object", + }, + { + "id": "destination_bowl", + "name": "bowl_ycb_robolab", + "params": {}, + "type": "object", + }, + ], + "tasks": [{ + "id": "task", + "params": { + "background_scene": "table", + "destination_location": task_destination, + "pick_up_object": task_pick, + }, + }], + }, + planner=planner, + ) + + +def _bundle(): + base_env = SimpleNamespace( + action_manager=_ActionManager(), + sim=SimpleNamespace(get_physics_dt=lambda: 0.005), + ) + adapter = SimpleNamespace(get_eef_names=lambda: ("franka",)) + return SimpleNamespace( + embodiment_adapter=adapter, + env=SimpleNamespace(unwrapped=base_env), + runtime_asset_evidence=_runtime_asset_evidence(), + success_contract_evidence={"attested": True, "predicate": "object_on_destination"}, + step_dt_s=0.02, + ) + + +def _runtime_asset_evidence(): + return { + "attested": True, + "attestation_scope": "runtime_usd_root_layer_identity_only", + "kinematic_frame_attestation": "separate_live_provider_attestation_required", + "motion_backend": "curobo_v1", + "override": "composed_scene.robot.spawn.usd_path_only", + "profile": "franka_ik_custream_v1_official_root_usd", + "reason": "pinned_official_isaac_5_1_root_layer_content_identity", + "referenced_usd_dependencies_attested": False, + "registry_usd_basename": "franka_panda_hand_on_stand.usd", + "registry_usd_path": _REGISTRY_PANDA_USD, + "root_layer": { + "attestation_method": "https_exact_url_sha256_v1", + "attested": True, + "bytes": _PINNED_PANDA_USD_BYTES, + "content_encoding": "identity", + "expected_bytes": _PINNED_PANDA_USD_BYTES, + "expected_sha256": _PINNED_PANDA_USD_SHA256, + "final_url": _PINNED_PANDA_USD, + "http_status": 200, + "max_bytes": 1 << 20, + "redirects_allowed": False, + "scope": "root_layer_bytes_only", + "sha256": _PINNED_PANDA_USD_SHA256, + "url": _PINNED_PANDA_USD, + }, + "runtime_uri_policy": "pinned_exact_https_no_redirect", + "runtime_usd_basename": "panda_instanceable.usd", + "runtime_usd_path": _PINNED_PANDA_USD, + "runtime_usd_release": "Isaac 5.1", + "schedulestream_application": "custream", + "schema_version": 2, + "semantic_embodiment": "franka_ik", + } + + +def _compatibility(): + identity = SimpleNamespace(source_commit="b" * 40, version="test") + return SimpleNamespace( + capabilities=SimpleNamespace(schedulestream=identity), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + +@pytest.mark.parametrize( + "registry_path", + [ + _REGISTRY_PANDA_USD, + "omniverse://custom-nucleus/Isaac/IsaacLab/Arena/assets/robot_library/franka_panda_hand_on_stand.usd", + ], +) +def test_episode_planner_passes_linked_task_pick_id_to_v1_factory(registry_path: str) -> None: + captured = {} + native = SimpleNamespace(close=lambda: None) + bundle = _bundle() + bundle.runtime_asset_evidence["registry_usd_path"] = registry_path + + def factory(env, predicates, **kwargs): + captured.update(env=env, predicates=predicates, kwargs=kwargs) + return native + + planner = create_schedulestream_episode_planner( + bundle, + _request(), + _compatibility(), + v1_factory=factory, + ) + + assert captured["kwargs"]["graspable_object"] == "pick_cube" + assert captured["kwargs"]["destination_object"] == "destination_bowl" + assert captured["kwargs"]["graspable_asset_name"] == "rubiks_cube_hot3d_robolab" + assert captured["kwargs"]["destination_asset_name"] == "bowl_ycb_robolab" + assert captured["predicates"][0].subject == "pick_cube" + assert planner._command_planner is native + assert planner._runtime_asset_evidence["runtime_usd_basename"] == "panda_instanceable.usd" + + +def test_episode_planner_rejects_missing_runtime_asset_attestation() -> None: + bundle = _bundle() + bundle.runtime_asset_evidence = {} + factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match="runtime asset schema-v2"): + create_schedulestream_episode_planner( + bundle, + _request(), + _compatibility(), + v1_factory=lambda *args, **kwargs: factory_calls.append((args, kwargs)), + ) + + assert factory_calls == [] + + +@pytest.mark.parametrize( + ("mutate", "message"), + [ + (lambda evidence: evidence.update(referenced_usd_dependencies_attested=True), "official-root"), + ( + lambda evidence: evidence.update( + registry_usd_path=_REGISTRY_PANDA_USD.replace( + "franka_panda_hand_on_stand.usd", "panda_instanceable.usd" + ) + ), + "registry USD path", + ), + (lambda evidence: evidence.update(registry_usd_basename="panda_instanceable.usd"), "official-root"), + (lambda evidence: evidence.update(registry_usd_path=_REGISTRY_PANDA_USD + "?version=1"), "registry USD path"), + ( + lambda evidence: evidence.update( + registry_usd_path="x" * 4_097 + "/Arena/assets/robot_library/franka_panda_hand_on_stand.usd" + ), + "registry USD path", + ), + (lambda evidence: evidence["root_layer"].update(sha256="0" * 64), "root layer"), + (lambda evidence: evidence["root_layer"].update(unreviewed=True), "root layer"), + ], +) +def test_episode_planner_rejects_inexact_runtime_asset_schema_v2(mutate, message: str) -> None: + bundle = _bundle() + mutate(bundle.runtime_asset_evidence) + factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match=message): + create_schedulestream_episode_planner( + bundle, + _request(), + _compatibility(), + v1_factory=lambda *args, **kwargs: factory_calls.append((args, kwargs)), + ) + + assert factory_calls == [] + + +def test_episode_planner_rejects_task_pick_and_goal_subject_mismatch() -> None: + factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match="does not exactly match"): + create_schedulestream_episode_planner( + _bundle(), + _request(task_pick="pick_cube", goal_subject="different_object"), + _compatibility(), + v1_factory=lambda *args, **kwargs: factory_calls.append((args, kwargs)), + ) + + assert factory_calls == [] + + +def test_episode_planner_rejects_task_destination_and_goal_target_mismatch() -> None: + factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match="does not exactly match"): + create_schedulestream_episode_planner( + _bundle(), + _request(goal_destination="different_bowl"), + _compatibility(), + v1_factory=lambda *args, **kwargs: factory_calls.append((args, kwargs)), + ) + + assert factory_calls == [] + + +def test_episode_planner_rejects_duplicate_linked_node_ids() -> None: + request = _request() + request.linked_graph["nodes"].append(dict(request.linked_graph["nodes"][1])) + factory_calls = [] + + with pytest.raises(ScheduleStreamProviderError, match="duplicated"): + create_schedulestream_episode_planner( + _bundle(), + request, + _compatibility(), + v1_factory=lambda *args, **kwargs: factory_calls.append((args, kwargs)), + ) + + assert factory_calls == [] diff --git a/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_goal_lowering.py b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_goal_lowering.py new file mode 100644 index 0000000..a090f18 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/schedulestream/test_goal_lowering.py @@ -0,0 +1,69 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import pytest + +from isaac_autodata_core.autonomous.task_motion import GoalPredicate +from isaac_autodata_interfaces.autonomous.schedulestream.goal_lowering import ( + GoalCompilationError, + ScheduleStreamGoalSymbols, + compile_schedulestream_goal, +) + + +class _Formula: + def __init__(self, text: str) -> None: + self.text = text + + def __and__(self, other: _Formula) -> _Formula: + return _Formula(f"({self.text} & {other.text})") + + +SYMBOLS = ScheduleStreamGoalSymbols( + attached_equals=lambda subject, target: _Formula(f"Attached({subject}) == {target}"), + holding_equals=lambda arm, subject: _Formula(f"Holding({arm}) == {subject}"), +) + + +def test_compiles_ordered_goal_conjunction(): + result = compile_schedulestream_goal( + ( + GoalPredicate("on", "cube_2", "cube_1"), + GoalPredicate("on", "cube_3", "cube_2"), + ), + SYMBOLS, + arm="panda_arm", + ) + + assert result.text == "(Attached(cube_2) == cube_1 & Attached(cube_3) == cube_2)" + + +def test_compiles_holding_goal_with_selected_arm(): + result = compile_schedulestream_goal( + (GoalPredicate("holding", "cube", None),), + SYMBOLS, + arm="panda_arm", + ) + + assert result.text == "Holding(panda_arm) == cube" + + +def test_rejects_capability_unsupported_relation(): + with pytest.raises(GoalCompilationError, match="not supported"): + compile_schedulestream_goal( + (GoalPredicate("in", "cube", "drawer"),), + SYMBOLS, + arm="panda_arm", + supported_relations=frozenset({"on"}), + ) + + +def test_rejects_invalid_predicate_arity_and_empty_goals(): + with pytest.raises(GoalCompilationError, match="requires target"): + compile_schedulestream_goal((GoalPredicate("on", "cube"),), SYMBOLS, arm="panda_arm") + with pytest.raises(GoalCompilationError, match="at least one"): + compile_schedulestream_goal((), SYMBOLS, arm="panda_arm") diff --git a/isaac_autodata_tests/interfaces/autonomous/test_arena_bridge.py b/isaac_autodata_tests/interfaces/autonomous/test_arena_bridge.py new file mode 100644 index 0000000..5591a1d --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_arena_bridge.py @@ -0,0 +1,332 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import copy +import random +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from isaac_autodata_interfaces.autonomous import ( + REQUIRED_ARENA_CAPABILITY, + REQUIRED_ARENA_COMMIT, + AutonomousValidationError, + CompilerTraceEvent, + LazyArenaIntentBridge, + build_arena_compilation_result, + compile_loaded_task_request, + load_task_request, + task_request_from_dict, +) + + +def _initial_graph() -> dict: + return { + "env_name": "llm_gen_maple_table_PickAndPlaceTask", + "nodes": [], + "tasks": [{"kind": "PickAndPlaceTask", "params": {}, "description": "place"}], + "initial_state_spec": { + "id": "state_initial", + "is_delta": False, + "spatial_constraints": [], + "task_constraints": [], + }, + } + + +def _linked_graph(selected: str = "pick_cube") -> dict: + return { + "env_name": "llm_gen_maple_table_PickAndPlaceTask", + "nodes": [], + "tasks": [ + { + "id": "task_0_PickAndPlaceTask", + "kind": "PickAndPlaceTask", + "params": {"pick_up_object": selected}, + "description": "place", + "initial_state_spec_id": "state_initial", + "success_state_spec_id": "state_spec_1", + }, + { + "id": "task_1_OpenDoorTask", + "kind": "OpenDoorTask", + "params": {}, + "description": "open", + "initial_state_spec_id": "state_spec_1", + "success_state_spec_id": "state_spec_2", + }, + ], + "state_specs": [ + { + "id": "state_initial", + "is_delta": False, + "spatial_constraints": [], + "task_constraints": [], + }, + { + "id": "state_spec_1", + "is_delta": True, + "spatial_constraints": [{ + "id": "state_spec_1_pick_cube_on_destination_bowl", + "kind": "on", + "subject": selected, + "reference": "destination_bowl", + "params": {"margin": 0.01}, + }], + "task_constraints": [], + }, + { + "id": "state_spec_2", + "is_delta": True, + "spatial_constraints": [], + "task_constraints": [], + }, + ], + "cli_override_specs": [], + } + + +def _envelope(tmp_path: Path): + return task_request_from_dict( + { + "schema_version": 1, + "name": "bridge_test", + "environment": {"intent": {"Arena": {"owns": ["all", "of", "this"]}}}, + "planner": { + "backend": "schedulestream", + "motion_backend": "curobo_v2", + "collisions": True, + "max_time_s": 30.0, + "batch_size": 64, + "interpolation_dt_s": 0.1, + "profile": False, + "animate": False, + }, + "generation": { + "successful_episodes": 2, + "seed": 11, + "num_envs": 1, + "max_attempts": 4, + }, + "output": { + "dataset": "outputs/data.hdf5", + "keep_failed": False, + "run_log": "outputs/data.jsonl", + }, + }, + source_path=tmp_path / "request.yaml", + ) + + +class _FakeBridge: + def __init__(self): + self.calls: list[tuple[dict, int]] = [] + + def compile_and_link(self, intent: dict, *, seed: int): + self.calls.append((copy.deepcopy(intent), seed)) + return build_arena_compilation_result( + _initial_graph(), + _linked_graph(), + [CompilerTraceEvent(stage="item.exact", query="cube", chosen="cube", note="")], + ) + + +def test_fake_bridge_resolves_source_free_request_and_absolute_outputs(tmp_path: Path): + envelope = _envelope(tmp_path) + bridge = _FakeBridge() + + resolved = compile_loaded_task_request(envelope, bridge=bridge) + + assert bridge.calls == [({"Arena": {"owns": ["all", "of", "this"]}}, 11)] + assert resolved.source_dataset_path is None + assert resolved.output.dataset == (tmp_path / "outputs" / "data.hdf5").resolve() + assert resolved.output.run_log == (tmp_path / "outputs" / "data.jsonl").resolve() + assert resolved.environment_name == "llm_gen_maple_table_PickAndPlaceTask" + assert len(resolved.request_digest) == 64 + assert len(resolved.graph_digest) == 64 + assert len(resolved.digest) == 64 + + +def test_goal_stages_follow_linked_task_and_constraint_order(): + result = build_arena_compilation_result(_initial_graph(), _linked_graph()) + + assert [stage.task_kind for stage in result.goal_stages] == ["PickAndPlaceTask", "OpenDoorTask"] + first, second = result.goal_stages + assert first.index == 0 + assert first.success_state_spec_id == "state_spec_1" + assert len(first.spatial_constraints) == 1 + constraint = first.spatial_constraints[0] + assert constraint.kind == "on" + assert constraint.subject == "pick_cube" + assert constraint.reference == "destination_bowl" + assert constraint.params == {"margin": 0.01} + assert second.spatial_constraints == () + + +def test_graph_digest_is_independent_of_mapping_key_order(): + first = _linked_graph() + second = dict(reversed(list(copy.deepcopy(first).items()))) + + result_a = build_arena_compilation_result(_initial_graph(), first) + result_b = build_arena_compilation_result(_initial_graph(), second) + + assert result_a.graph_digest == result_b.graph_digest + assert result_a.linked_graph_json == result_b.linked_graph_json + + +def test_resolved_request_is_deterministic_for_same_fake_bridge(tmp_path: Path): + envelope = _envelope(tmp_path) + + first = compile_loaded_task_request(envelope, bridge=_FakeBridge()) + second = compile_loaded_task_request(envelope, bridge=_FakeBridge()) + + assert first.canonical_json() == second.canonical_json() + assert first.digest == second.digest + + +def test_missing_success_state_is_a_structured_graph_contract_error(): + linked = _linked_graph() + linked["tasks"][0]["success_state_spec_id"] = "missing" + + with pytest.raises(AutonomousValidationError) as exc: + build_arena_compilation_result(_initial_graph(), linked) + + issue = exc.value.issues[0] + assert issue.code == "arena_graph_contract_error" + assert issue.field_path == "$.arena.linked_graph.tasks[0].success_state_spec_id" + + +def test_unexpected_fake_bridge_failure_is_structured(tmp_path: Path): + class BrokenBridge: + def compile_and_link(self, intent: dict, *, seed: int): + raise RuntimeError("backend exploded\nsecret second line") + + with pytest.raises(AutonomousValidationError) as exc: + compile_loaded_task_request(_envelope(tmp_path), bridge=BrokenBridge()) + + issue = exc.value.issues[0] + assert issue.code == "arena_bridge_failed" + assert issue.field_path == "$.environment.intent" + assert "Traceback" not in issue.message + assert "\n" not in issue.message + + +def test_missing_arena_api_error_names_required_capability_and_commit(): + def missing(_name: str): + raise ModuleNotFoundError("old Arena checkout") + + bridge = LazyArenaIntentBridge(module_loader=missing) + with pytest.raises(AutonomousValidationError) as exc: + bridge.compile_and_link({}, seed=1) + + issue = exc.value.issues[0] + assert issue.code == "arena_intent_api_unavailable" + assert REQUIRED_ARENA_CAPABILITY in issue.message + assert REQUIRED_ARENA_COMMIT in issue.message + assert "Traceback" not in issue.message + + +class _FakeIntentSpec: + @classmethod + def model_validate(cls, value: dict): + random.random() + return copy.deepcopy(value) + + +class _FakeInitialGraphModel: + def __init__(self, selected: str): + self.selected = selected + + def to_dict(self): + value = _initial_graph() + value["selected"] = self.selected + return value + + def link(self): + return _FakeLinkedGraphModel(self.selected) + + +class _FakeLinkedGraphModel: + def __init__(self, selected: str): + self.selected = selected + + def to_dict(self): + return _linked_graph(self.selected) + + +class _FakeIntentCompiler: + def __init__(self): + self.trace = [] + self.resolution_errors = [] + + def compile(self, _spec): + selected = random.choice(["cube_1", "cube_2", "cube_3"]) + self.trace.append(SimpleNamespace(stage="task.resolved_param", query="cube", chosen=selected, note="")) + return _FakeInitialGraphModel(selected) + + +def _fake_module_loader(name: str): + if name.endswith("environment_intent_spec"): + return SimpleNamespace(EnvironmentIntentSpec=_FakeIntentSpec) + if name.endswith("intent_compiler"): + return SimpleNamespace(IntentCompiler=_FakeIntentCompiler) + raise ModuleNotFoundError(name) + + +def test_lazy_bridge_seeds_and_restores_global_random_state(): + bridge = LazyArenaIntentBridge(module_loader=_fake_module_loader) + random.seed(123456) + state_before = random.getstate() + + first = bridge.compile_and_link({"opaque": True}, seed=42) + state_after = random.getstate() + second = bridge.compile_and_link({"opaque": True}, seed=42) + + assert state_after == state_before + assert random.getstate() == state_before + assert first.linked_graph == second.linked_graph + assert first.compiler_trace == second.compiler_trace + + +def test_resolution_error_fails_and_restores_random_state(): + class ErrorCompiler(_FakeIntentCompiler): + def compile(self, spec): + model = super().compile(spec) + self.resolution_errors = [ + SimpleNamespace(stage="item.required_tags.miss", query="cube", chosen=None, note="not found") + ] + return model + + def loader(name: str): + if name.endswith("environment_intent_spec"): + return SimpleNamespace(EnvironmentIntentSpec=_FakeIntentSpec) + return SimpleNamespace(IntentCompiler=ErrorCompiler) + + bridge = LazyArenaIntentBridge(module_loader=loader) + random.seed(9876) + state_before = random.getstate() + + with pytest.raises(AutonomousValidationError) as exc: + bridge.compile_and_link({"opaque": True}, seed=42) + + assert exc.value.issues[0].code == "arena_resolution_error" + assert "item.required_tags.miss" in exc.value.issues[0].message + assert random.getstate() == state_before + + +def test_default_bridge_live_example_when_current_arena_api_is_available(): + bridge = LazyArenaIntentBridge() + if not bridge.is_available(): + pytest.skip("host Arena install predates the required agentic intent API") + example = Path(__file__).parents[3] / "isaac_autodata_examples" / "autonomous" / "franka_pick_cube_into_bowl.yaml" + request = load_task_request(example) + + result = bridge.compile_and_link(request.environment_intent, seed=request.generation.seed) + + assert result.linked_graph["tasks"] + assert result.goal_stages[0].task_kind == "PickAndPlaceTask" diff --git a/isaac_autodata_tests/interfaces/autonomous/test_arena_environment.py b/isaac_autodata_tests/interfaces/autonomous/test_arena_environment.py new file mode 100644 index 0000000..61d8d09 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_arena_environment.py @@ -0,0 +1,519 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import hashlib +import io +import os +import urllib.error +from types import SimpleNamespace + +import pytest + +from isaac_autodata_core.autonomous.output_transaction import RecordingTargets +from isaac_autodata_interfaces.autonomous.arena_environment import ( + ArenaRuntimeBundle, + _attest_exact_url_content, + apply_custream_v1_runtime_asset_profile, + attest_custream_v1_runtime_usd_root, + attest_pick_and_place_success_contract, + build_arena_runtime, + goal_predicates_from_request, + make_embodiment_adapter, + validate_planner_timing, +) + +_PINNED_PANDA_USD = ( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/5.1/Isaac/IsaacLab/" + "Robots/FrankaEmika/panda_instanceable.usd" +) +_PINNED_PANDA_USD_BYTES = 8_038 +_PINNED_PANDA_USD_SHA256 = "7f5a0c0aa6760cfbd348e08bc464d4b94341f027f51c2d9e42406ceefcc7787f" +_MAX_RUNTIME_USD_BYTES = 1 << 20 + + +def _root_layer_evidence(**overrides): + evidence = { + "attestation_method": "https_exact_url_sha256_v1", + "attested": True, + "bytes": _PINNED_PANDA_USD_BYTES, + "content_encoding": "identity", + "expected_bytes": _PINNED_PANDA_USD_BYTES, + "expected_sha256": _PINNED_PANDA_USD_SHA256, + "final_url": _PINNED_PANDA_USD, + "http_status": 200, + "max_bytes": _MAX_RUNTIME_USD_BYTES, + "redirects_allowed": False, + "scope": "root_layer_bytes_only", + "sha256": _PINNED_PANDA_USD_SHA256, + "url": _PINNED_PANDA_USD, + } + evidence.update(overrides) + return evidence + + +class _Response: + def __init__(self, payload, *, url, status=200, headers=None): + self._payload = io.BytesIO(payload) + self._url = url + self.status = status + self.headers = {"Content-Length": str(len(payload))} if headers is None else headers + + def __enter__(self): + return self + + def __exit__(self, *_args): + self._payload.close() + + def geturl(self): + return self._url + + def read(self, size): + return self._payload.read(size) + + +class _CloseCountingEnvironment: + def __init__(self): + self.close_calls = 0 + + def close(self): + self.close_calls += 1 + + +def _request(*, embodiment_name="franka_ik"): + constraint = SimpleNamespace(kind="on", subject="cube", reference="bowl") + stage = SimpleNamespace(spatial_constraints=(constraint,)) + return SimpleNamespace( + goal_stages=(stage,), + linked_graph={"nodes": [{"id": "robot", "name": embodiment_name, "type": "embodiment"}]}, + ) + + +def test_goal_projection_preserves_linked_scene_ids(): + predicates = goal_predicates_from_request(_request()) + + assert [predicate.to_dict() for predicate in predicates] == [ + {"relation": "on", "subject": "cube", "target": "bowl"} + ] + + +def test_empty_arena_goal_is_rejected(): + with pytest.raises(ValueError, match="no spatial success constraints"): + goal_predicates_from_request(SimpleNamespace(goal_stages=())) + + +def test_franka_runtime_binding_is_explicit(): + adapter = make_embodiment_adapter(_request()) + + assert adapter.name == "arena_franka_ik" + assert adapter.get_eef_names() == ("franka",) + assert adapter.gripper_action_dim == 1 + assert adapter.eef_offset == pytest.approx((0.0, 0.0, -0.0036)) + + +def test_unknown_embodiment_does_not_silently_reuse_franka_profile(): + with pytest.raises(NotImplementedError, match="droid"): + make_embodiment_adapter(_request(embodiment_name="droid")) + + +def _scene_robot_cfg(usd_basename: str = "franka_panda_hand_on_stand.usd"): + return SimpleNamespace(robot=SimpleNamespace(spawn=SimpleNamespace(usd_path=f"omniverse://arena/{usd_basename}"))) + + +def test_custream_v1_runtime_asset_profile_substitutes_only_reviewed_usd(): + scene_cfg = _scene_robot_cfg() + runtime_usd_path = _PINNED_PANDA_USD + attested_urls = [] + + def attest_root_layer(url): + attested_urls.append(url) + return _root_layer_evidence() + + evidence = apply_custream_v1_runtime_asset_profile( + _request(), + scene_cfg, + runtime_usd_path=runtime_usd_path, + root_layer_attestor=attest_root_layer, + ) + + assert attested_urls == [_PINNED_PANDA_USD] + assert scene_cfg.robot.spawn.usd_path == runtime_usd_path + assert evidence == { + "attested": True, + "attestation_scope": "runtime_usd_root_layer_identity_only", + "kinematic_frame_attestation": "separate_live_provider_attestation_required", + "motion_backend": "curobo_v1", + "override": "composed_scene.robot.spawn.usd_path_only", + "profile": "franka_ik_custream_v1_official_root_usd", + "reason": "pinned_official_isaac_5_1_root_layer_content_identity", + "referenced_usd_dependencies_attested": False, + "registry_usd_basename": "franka_panda_hand_on_stand.usd", + "registry_usd_path": "omniverse://arena/franka_panda_hand_on_stand.usd", + "root_layer": _root_layer_evidence(), + "runtime_usd_basename": "panda_instanceable.usd", + "runtime_usd_path": runtime_usd_path, + "runtime_usd_release": "Isaac 5.1", + "runtime_uri_policy": "pinned_exact_https_no_redirect", + "schedulestream_application": "custream", + "schema_version": 2, + "semantic_embodiment": "franka_ik", + } + + +def test_runtime_asset_profile_fails_closed_before_mutation_when_root_attestation_fails(): + scene_cfg = _scene_robot_cfg() + original_path = scene_cfg.robot.spawn.usd_path + + def fail_attestation(_url): + raise ValueError("offline or mismatched") + + with pytest.raises(ValueError, match="root-layer attestation failed"): + apply_custream_v1_runtime_asset_profile( + _request(), + scene_cfg, + runtime_usd_path=_PINNED_PANDA_USD, + root_layer_attestor=fail_attestation, + ) + + assert scene_cfg.robot.spawn.usd_path == original_path + + +def test_runtime_asset_profile_rejects_incomplete_injected_root_evidence(): + scene_cfg = _scene_robot_cfg() + original_path = scene_cfg.robot.spawn.usd_path + + with pytest.raises(ValueError, match="incomplete or mismatched"): + apply_custream_v1_runtime_asset_profile( + _request(), + scene_cfg, + runtime_usd_path=_PINNED_PANDA_USD, + root_layer_attestor=lambda _url: _root_layer_evidence(sha256="0" * 64), + ) + + assert scene_cfg.robot.spawn.usd_path == original_path + + +def test_exact_url_content_attestation_hashes_bounded_identity_response_without_network(): + url = "https://assets.example.test/root.usd" + payload = b"#usda 1.0\n" + opened = [] + + def opener(request, timeout_s): + opened.append((request, timeout_s)) + return _Response(payload, url=url) + + evidence = _attest_exact_url_content( + url, + expected_url=url, + expected_sha256=hashlib.sha256(payload).hexdigest(), + expected_bytes=len(payload), + max_bytes=64, + timeout_s=2.0, + opener=opener, + ) + + assert len(opened) == 1 + assert opened[0][0].full_url == url + assert opened[0][0].get_header("Accept-encoding") == "identity" + assert opened[0][1] == pytest.approx(2.0) + assert evidence["attested"] is True + assert evidence["scope"] == "root_layer_bytes_only" + assert evidence["sha256"] == hashlib.sha256(payload).hexdigest() + + +def test_exact_url_content_attestation_rejects_redirect_or_final_url_change_without_network(): + url = "https://assets.example.test/root.usd" + payload = b"root" + digest = hashlib.sha256(payload).hexdigest() + + def redirect_error(request, _timeout_s): + raise urllib.error.HTTPError(request.full_url, 302, "redirect", {"Location": url + "?mirror=1"}, None) + + with pytest.raises(ValueError, match="redirects are forbidden"): + _attest_exact_url_content( + url, + expected_url=url, + expected_sha256=digest, + expected_bytes=len(payload), + max_bytes=64, + timeout_s=2.0, + opener=redirect_error, + ) + + with pytest.raises(ValueError, match="final-URL changes are forbidden"): + _attest_exact_url_content( + url, + expected_url=url, + expected_sha256=digest, + expected_bytes=len(payload), + max_bytes=64, + timeout_s=2.0, + opener=lambda *_args: _Response(payload, url=url + "?mirror=1"), + ) + + +@pytest.mark.parametrize( + ("headers", "payload"), + [ + ({"Content-Length": "65"}, b"root"), + ({}, b"x" * 65), + ], +) +def test_exact_url_content_attestation_rejects_declared_or_streamed_oversize(headers, payload): + url = "https://assets.example.test/root.usd" + + with pytest.raises(ValueError, match="exceeds the 64-byte download bound"): + _attest_exact_url_content( + url, + expected_url=url, + expected_sha256=hashlib.sha256(b"root").hexdigest(), + expected_bytes=4, + max_bytes=64, + timeout_s=2.0, + opener=lambda *_args: _Response(payload, url=url, headers=headers), + ) + + +def test_exact_url_content_attestation_rejects_size_or_sha256_mismatch(): + url = "https://assets.example.test/root.usd" + payload = b"root" + + with pytest.raises(ValueError, match="byte count mismatch"): + _attest_exact_url_content( + url, + expected_url=url, + expected_sha256=hashlib.sha256(payload).hexdigest(), + expected_bytes=5, + max_bytes=64, + timeout_s=2.0, + opener=lambda *_args: _Response(payload, url=url), + ) + + with pytest.raises(ValueError, match="SHA-256"): + _attest_exact_url_content( + url, + expected_url=url, + expected_sha256="0" * 64, + expected_bytes=len(payload), + max_bytes=64, + timeout_s=2.0, + opener=lambda *_args: _Response(payload, url=url), + ) + + +def test_official_root_attestor_uses_fixed_size_and_digest_without_network(): + payload = b"x" * _PINNED_PANDA_USD_BYTES + + with pytest.raises(ValueError, match="SHA-256"): + attest_custream_v1_runtime_usd_root( + _PINNED_PANDA_USD, + opener=lambda *_args: _Response(payload, url=_PINNED_PANDA_USD), + ) + + +@pytest.mark.parametrize( + ("resolved_request", "scene_cfg", "runtime_usd_path", "message"), + [ + ( + _request(embodiment_name="droid"), + _scene_robot_cfg(), + _PINNED_PANDA_USD, + "franka_ik", + ), + ( + _request(), + _scene_robot_cfg("unreviewed_robot.usd"), + _PINNED_PANDA_USD, + "registry asset changed", + ), + ( + _request(), + _scene_robot_cfg(), + "omniverse://isaaclab/Robots/FrankaEmika/unreviewed_robot.usd", + "runtime asset changed", + ), + ( + _request(), + _scene_robot_cfg(), + "https://unreviewed.invalid/Robots/FrankaEmika/panda_instanceable.usd", + "pinned official production URI", + ), + ], +) +def test_custream_v1_runtime_asset_profile_rejects_semantic_or_asset_drift( + resolved_request, + scene_cfg, + runtime_usd_path, + message, +): + original_path = scene_cfg.robot.spawn.usd_path + + with pytest.raises(ValueError, match=message): + apply_custream_v1_runtime_asset_profile( + resolved_request, + scene_cfg, + runtime_usd_path=runtime_usd_path, + ) + + assert scene_cfg.robot.spawn.usd_path == original_path + + +def test_planner_timing_requires_exact_environment_cadence(): + validate_planner_timing(0.05, 0.050000001) + + with pytest.raises(ValueError, match="must match"): + validate_planner_timing(0.05, 0.02) + + +def check_success(): + pass + + +def object_on_destination(): + pass + + +check_success.__module__ = "isaaclab_arena.tasks.terminations" +object_on_destination.__module__ = "isaaclab_arena.tasks.terminations" + + +def _success_contract( + *, + force_threshold=0.1, + sensor_path="{ENV_REGEX_NS}/cube", + filter_path="{ENV_REGEX_NS}/bowl", +): + predicate = SimpleNamespace( + func=object_on_destination, + params={ + "contact_sensor_cfg": SimpleNamespace(name="pick_up_object_contact_sensor"), + "force_threshold": force_threshold, + "object_cfg": SimpleNamespace(name="cube"), + "velocity_threshold": 0.1, + }, + ) + success = SimpleNamespace(func=check_success, params={"mode": "ALL", "predicates": [predicate]}) + scene = SimpleNamespace( + pick_up_object_contact_sensor=SimpleNamespace( + prim_path=sensor_path, + filter_prim_paths_expr=[filter_path], + ) + ) + return success, scene + + +def test_success_contract_attests_exact_task_sensor_and_thresholds(): + success, scene = _success_contract() + + evidence = attest_pick_and_place_success_contract( + success, + scene, + pick_up_object="cube", + destination_location="bowl", + ) + + assert evidence["attested"] is True + assert evidence["subject"] == "cube" + assert evidence["force_threshold_n"] == pytest.approx(0.1) + + +def test_success_contract_rejects_threshold_or_contact_filter_drift(): + success, scene = _success_contract(force_threshold=0.2) + with pytest.raises(ValueError, match="force threshold"): + attest_pick_and_place_success_contract( + success, + scene, + pick_up_object="cube", + destination_location="bowl", + ) + + success, scene = _success_contract(filter_path="{ENV_REGEX_NS}/table") + with pytest.raises(ValueError, match="destination object"): + attest_pick_and_place_success_contract( + success, + scene, + pick_up_object="cube", + destination_location="bowl", + ) + + +@pytest.mark.parametrize( + ("sensor_path", "filter_path", "message"), + [ + ("{ENV_REGEX_NS}/not_cube", "{ENV_REGEX_NS}/bowl", "prim path"), + ("{ENV_REGEX_NS}/cube", "{ENV_REGEX_NS}/not_bowl", "destination object"), + ("/World/envs/env_.*/cube", "{ENV_REGEX_NS}/bowl", "prim path"), + ], +) +def test_success_contract_requires_exact_safe_object_paths(sensor_path, filter_path, message): + success, scene = _success_contract(sensor_path=sensor_path, filter_path=filter_path) + + with pytest.raises(ValueError, match=message): + attest_pick_and_place_success_contract( + success, + scene, + pick_up_object="cube", + destination_location="bowl", + ) + + +def _runtime_request(dataset, *, keep_failed=False): + return SimpleNamespace( + generation=SimpleNamespace(num_envs=1), + output=SimpleNamespace(dataset=dataset, keep_failed=keep_failed), + ) + + +def test_runtime_rejects_existing_dataset_before_heavy_imports(tmp_path): + dataset = tmp_path / "output.hdf5" + dataset.write_bytes(b"do not overwrite") + + with pytest.raises(FileExistsError, match="overwrite"): + build_arena_runtime(_runtime_request(dataset), SimpleNamespace()) + + +def test_runtime_rejects_existing_failed_dataset_before_heavy_imports(tmp_path): + dataset = tmp_path / "output.hdf5" + (tmp_path / "output_failed.hdf5").write_bytes(b"do not overwrite") + + with pytest.raises(FileExistsError, match="failed"): + build_arena_runtime(_runtime_request(dataset, keep_failed=True), SimpleNamespace()) + + +def test_runtime_rejects_broken_dataset_symlink_before_heavy_imports(tmp_path): + dataset = tmp_path / "output.hdf5" + dataset.symlink_to(tmp_path / "missing.hdf5") + + with pytest.raises(FileExistsError, match="overwrite"): + build_arena_runtime(_runtime_request(dataset), SimpleNamespace()) + + +def test_runtime_requires_recording_override_to_preserve_dataset_stem(tmp_path): + dataset = tmp_path / "output.hdf5" + descriptor = os.open(tmp_path, os.O_RDONLY | os.O_DIRECTORY) + try: + targets = RecordingTargets( + dataset_export_dir_path=f"/proc/self/fd/{descriptor}", + dataset_filename="redirected", + ) + with pytest.raises(ValueError, match="preserve"): + build_arena_runtime( + _runtime_request(dataset), + SimpleNamespace(), + recording_targets=targets, + ) + finally: + os.close(descriptor) + + +def test_runtime_bundle_close_is_idempotent(): + env = _CloseCountingEnvironment() + bundle = ArenaRuntimeBundle(env, object(), object(), object(), 0.02) + + bundle.close() + bundle.close() + + assert env.close_calls == 1 diff --git a/isaac_autodata_tests/interfaces/autonomous/test_isaaclab_runtime.py b/isaac_autodata_tests/interfaces/autonomous/test_isaaclab_runtime.py new file mode 100644 index 0000000..89894da --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_isaaclab_runtime.py @@ -0,0 +1,2088 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import asyncio +import json +import math +import torch +from dataclasses import replace +from types import SimpleNamespace + +import pytest + +from isaac_autodata_core.autonomous.attempt_generation import AttemptRequest, FailureStage +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + DetachIntentSegment, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + JointTrajectorySegment, + TaskMotionPlan, +) +from isaac_autodata_interfaces.autonomous.isaaclab_runtime import ( + AttachmentState, + IsaacLabAttemptRuntime, + IsaacLabPlanExecutor, +) + + +def _identity(x: float = 0.0): + return ( + (1.0, 0.0, 0.0, x), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), + ) + + +def _yaw(angle_rad: float): + cosine = math.cos(angle_rad) + sine = math.sin(angle_rad) + return ( + (cosine, -sine, 0.0, 0.0), + (sine, cosine, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), + ) + + +def _translation(x: float, y: float, z: float): + return ( + (1.0, 0.0, 0.0, x), + (0.0, 1.0, 0.0, y), + (0.0, 0.0, 1.0, z), + (0.0, 0.0, 0.0, 1.0), + ) + + +class _Recorder: + def __init__(self): + self.data = [] + self.reset_ids = None + self.reset_calls = [] + self.post_reset_calls = [] + self.initial_state_supplier = lambda: "settled_state" + self.success = None + self.export_ids = None + self.export_calls = 0 + + def reset(self, env_ids): + self.reset_ids = env_ids.clone() + self.reset_calls.append(env_ids.clone()) + self.data = [] + + def record_post_reset(self, env_ids): + self.post_reset_calls.append(env_ids.clone()) + self.data.append(("initial_state", self.initial_state_supplier())) + + def set_success_to_episodes(self, env_ids, success): + self.success = (env_ids.clone(), success.clone()) + + def export_episodes(self, env_ids): + self.export_calls += 1 + self.export_ids = env_ids.clone() + + +class _Env: + def __init__(self): + obj_data = SimpleNamespace( + root_pos_w=torch.tensor([[1.0, 2.0, 3.0]]), + root_quat_w=torch.tensor([[0.0, 0.0, 0.0, 1.0]]), + ) + obj_cfg = SimpleNamespace(spawn=SimpleNamespace(usd_path="asset.usd")) + robot_data = SimpleNamespace( + root_pos_w=torch.tensor([[1.0, 2.0, 3.0]]), + root_quat_w=torch.tensor([[0.0, 0.0, 0.0, 1.0]]), + ) + self.scene = SimpleNamespace( + env_origins=torch.tensor([[1.0, 2.0, 3.0]]), + articulations={"robot": SimpleNamespace(data=robot_data)}, + rigid_objects={"cube": SimpleNamespace(data=obj_data, cfg=obj_cfg)}, + ) + self.action_space = SimpleNamespace(shape=(1, 7)) + self.num_envs = 1 + self.device = "cpu" + self.step_dt = 0.05 + self.recorder_manager = _Recorder() + self.reset_calls = [] + self.step_calls = [] + + @property + def unwrapped(self): + return self + + def reset(self, env_ids): + self.reset_calls.append(env_ids.clone()) + + def step(self, action): + self.step_calls.append(action.clone()) + self.recorder_manager.data.append(("action", action.clone())) + + +class _Adapter: + name = "fake_robot" + gripper_action_dim = 1 + + def __init__(self): + self.env = None + self.pose = torch.eye(4).unsqueeze(0) + self.grippers = [] + + def bind_env(self, env): + self.env = env + + def get_eef_names(self): + return ("tool",) + + def get_joint_positions(self, env_ids): + return torch.tensor([[0.1, 0.2]]) + + def get_joint_names(self): + return ["j1", "j2"] + + def get_eef_poses(self, env_ids): + return {"tool": self.pose.clone()} + + def target_eef_pose_to_action( + self, + target_eef_pose_dict, + gripper_action_dict, + action_noise_dict, + env_id, + ): + self.pose[0] = target_eef_pose_dict["tool"] + self.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + +def _request(): + return AttemptRequest( + request_digest="request", + attempt_index=0, + seed=3, + env_id=0, + goal=(GoalPredicate("on", "cube", "table"),), + keep_failed=False, + ) + + +def _plan(*segments): + return TaskMotionPlan( + plan_id="plan", + request_digest="request", + snapshot_digest="snapshot", + backend="fake", + backend_version="1", + seed=3, + segments=segments, + goal=(GoalPredicate("on", "cube", "table"),), + ) + + +def test_runtime_reset_snapshot_and_successful_export(): + env = _Env() + adapter = _Adapter() + env.recorder_manager.initial_state_supplier = lambda: {"simulator_steps": len(env.step_calls)} + attachment_state = AttachmentState() + runtime = IsaacLabAttemptRuntime( + env, + adapter, + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + attachment_state=attachment_state, + ) + + reset_result = asyncio.run(runtime.reset_attempt(0)) + snapshot = runtime.capture_scene_snapshot(0, snapshot_id="snapshot") + asyncio.run(runtime.finish_attempt(0, success=True, keep_failed=False)) + + assert reset_result == { + "env_id": 0, + "recorder_excludes_reset_settling": True, + "recorder_initial_state_recaptured_after_settling": True, + "reset_completed": True, + "reset_settle_duration_s": 0.5, + "reset_settle_steps": 10, + } + assert snapshot.robot.robot_id == "robot" + assert snapshot.objects[0].pose == _identity() + assert snapshot.objects[0].geometry_ref == "asset.usd" + assert snapshot.metadata["rigid_object_quaternion_convention"] == "xyzw" + assert len(env.step_calls) == 10 + assert len(env.recorder_manager.reset_calls) == 2 + assert len(env.recorder_manager.post_reset_calls) == 1 + assert env.recorder_manager.data == [("initial_state", {"simulator_steps": 10})] + assert adapter.grippers == [1.0] * 10 + assert env.recorder_manager.success[1].item() + assert env.recorder_manager.export_ids.tolist() == [0] + + +def test_runtime_zero_settle_keeps_env_reset_initial_state_without_manual_recapture(): + class _ResetRecordingEnv(_Env): + def reset(self, env_ids): + super().reset(env_ids) + self.recorder_manager.reset(env_ids) + self.recorder_manager.record_post_reset(env_ids) + + env = _ResetRecordingEnv() + env.recorder_manager.initial_state_supplier = lambda: {"simulator_steps": len(env.step_calls)} + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + reset_settle_steps=0, + ) + + reset_result = asyncio.run(runtime.reset_attempt(0)) + + assert reset_result["recorder_excludes_reset_settling"] is True + assert reset_result["recorder_initial_state_recaptured_after_settling"] is False + assert len(env.recorder_manager.post_reset_calls) == 1 + assert env.recorder_manager.data == [("initial_state", {"simulator_steps": 0})] + assert env.step_calls == [] + + +def test_runtime_settled_initial_state_precedes_next_task_sample(): + env = _Env() + env.recorder_manager.initial_state_supplier = lambda: {"simulator_steps": len(env.step_calls)} + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + + asyncio.run(runtime.reset_attempt(0)) + task_action = torch.zeros(env.action_space.shape) + env.step(task_action) + + assert env.recorder_manager.data[0] == ("initial_state", {"simulator_steps": 10}) + assert env.recorder_manager.data[1][0] == "action" + assert torch.equal(env.recorder_manager.data[1][1], task_action) + + +def test_runtime_initial_state_recapture_failure_propagates_after_clearing_setup_samples(): + env = _Env() + recorder = env.recorder_manager + + def fail_recapture(env_ids): + recorder.post_reset_calls.append(env_ids.clone()) + raise RuntimeError("settled initial-state capture failed") + + recorder.record_post_reset = fail_recapture + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + + with pytest.raises(RuntimeError, match="settled initial-state capture failed"): + asyncio.run(runtime.reset_attempt(0)) + + assert len(env.step_calls) == 10 + assert len(recorder.reset_calls) == 2 + assert len(recorder.post_reset_calls) == 1 + assert recorder.data == [] + + +def test_runtime_snapshot_decodes_isaaclab_xyzw_object_quaternion(): + env = _Env() + half_angle = math.pi / 4 + env.scene.rigid_objects["cube"].data.root_quat_w = torch.tensor( + [[0.0, 0.0, math.sin(half_angle), math.cos(half_angle)]] + ) + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + reset_settle_steps=0, + ) + + snapshot = runtime.capture_scene_snapshot(0, snapshot_id="snapshot") + + assert torch.allclose(torch.tensor(snapshot.objects[0].pose), torch.tensor(_yaw(math.pi / 2)), atol=1e-6) + + +@pytest.mark.parametrize("reset_settle_steps", [True, -1, 101]) +def test_runtime_rejects_unbounded_reset_settling(reset_settle_steps): + with pytest.raises(ValueError, match="reset_settle_steps"): + IsaacLabAttemptRuntime( + _Env(), + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + reset_settle_steps=reset_settle_steps, + ) + + +def test_runtime_reset_settling_rejects_saturated_action_before_simulator_step(): + class _SaturatedAdapter(_Adapter): + def target_eef_pose_to_action(self, *args, **kwargs): + del args, kwargs + return torch.tensor([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + + env = _Env() + runtime = IsaacLabAttemptRuntime( + env, + _SaturatedAdapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + + with pytest.raises(ValueError, match="clipping boundary"): + asyncio.run(runtime.reset_attempt(0)) + + assert env.step_calls == [] + assert len(env.recorder_manager.reset_calls) == 1 + + +def test_failed_episode_is_not_exported_when_retention_is_disabled(): + env = _Env() + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + + asyncio.run(runtime.finish_attempt(0, success=False, keep_failed=False)) + + assert env.recorder_manager.export_ids is None + + +def test_attempt_finalization_is_idempotent_and_rejects_conflicting_retries(): + env = _Env() + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + + asyncio.run(runtime.finish_attempt(0, success=True, keep_failed=False)) + asyncio.run(runtime.finish_attempt(0, success=True, keep_failed=False)) + + assert env.recorder_manager.export_calls == 1 + with pytest.raises(RuntimeError, match="different retention state"): + asyncio.run(runtime.finish_attempt(0, success=False, keep_failed=False)) + + +def test_failed_second_attempt_reset_clears_prior_finalization_before_failure(): + env = _Env() + runtime = IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + ), + ) + asyncio.run(runtime.finish_attempt(0, success=True, keep_failed=False)) + + def fail_reset(*, env_ids): + del env_ids + raise RuntimeError("second attempt reset failed") + + env.reset = fail_reset + with pytest.raises(RuntimeError, match="second attempt reset failed"): + asyncio.run(runtime.reset_attempt(0)) + + asyncio.run(runtime.finish_attempt(0, success=False, keep_failed=False)) + asyncio.run(runtime.finish_attempt(0, success=False, keep_failed=False)) + + assert env.recorder_manager.export_calls == 1 + assert env.recorder_manager.success[1].item() is False + with pytest.raises(RuntimeError, match="different retention state"): + asyncio.run(runtime.finish_attempt(0, success=True, keep_failed=False)) + + +def test_runtime_rejects_unbound_semantic_objects_before_reset(): + env = _Env() + + with pytest.raises(ValueError, match="missing_bowl"): + IsaacLabAttemptRuntime( + env, + _Adapter(), + graph_nodes=( + {"id": "robot", "type": "embodiment"}, + {"id": "cube", "type": "object"}, + {"id": "missing_bowl", "type": "object"}, + ), + ) + + +def test_executor_runs_cartesian_gripper_and_attachment_then_verifies_final_state(): + env = _Env() + adapter = _Adapter() + attachment_state = AttachmentState() + attachment_state.reset(("tool",)) + segments = ( + CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1), _identity(0.2)), + ), + GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ), + AttachIntentSegment( + segment_id="attach", + depends_on=("close",), + eef_name="tool", + object_name="cube", + verifier="test", + ), + ) + verifier_calls = [] + + def verifier(_env, env_id): + verifier_calls.append(env_id) + return torch.tensor([True]) + + executor = IsaacLabPlanExecutor( + env, + adapter, + verifier, + attachment_state=attachment_state, + attachment_verifier=lambda *_: True, + final_settle_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(*segments))) + + assert result.success + assert verifier_calls == [0, 0, 0] + assert attachment_state.held_by_eef == {"tool": "cube"} + assert result.final_observation["steps"] == 5 + assert adapter.grippers == [1.0, 1.0, -1.0, -1.0, -1.0] + + +def test_executor_rejects_false_final_state_without_or_accumulation(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: False, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "final_goal_not_satisfied" + + +def test_executor_rejects_joint_trajectory_for_ik_adapter_as_unrecoverable(): + env = _Env() + adapter = _Adapter() + segment = JointTrajectorySegment( + segment_id="joint", + joint_names=("j1",), + positions=((0.0,),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_stage is FailureStage.EXECUTION + assert result.failure_code == "joint_trajectory_not_supported_by_ik_executor" + assert not result.recoverable + + +def test_executor_rejects_unsupported_late_segment_before_any_action(): + env = _Env() + adapter = _Adapter() + cartesian = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + joint = JointTrajectorySegment( + segment_id="joint", + depends_on=("move",), + joint_names=("j1",), + positions=((0.0,),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(cartesian, joint))) + + assert not result.success + assert result.failure_code == "joint_trajectory_not_supported_by_ik_executor" + assert env.step_calls == [] + + +def test_executor_rejects_non_topological_segment_order_before_any_action(): + env = _Env() + adapter = _Adapter() + move = CartesianTrajectorySegment( + segment_id="move", + depends_on=("close",), + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + close = GripperCommandSegment( + segment_id="close", + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert not result.success + assert result.failure_code == "plan_not_topologically_ordered" + assert env.step_calls == [] + + +def test_executor_rejects_over_budget_plan_before_any_action(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1), _identity(0.2)), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0, max_steps=1) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "execution_step_limit" + assert env.step_calls == [] + + +def test_executor_rejects_eef_tracking_divergence_after_bounded_corrections(): + env = _Env() + adapter = _Adapter() + + def action_without_observation_update(*_args, **_kwargs): + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_without_observation_update + + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_eef_position_error_m=0.01, + max_tracking_correction_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "eef_position_tracking_error" + assert len(env.step_calls) == 3 + assert result.final_observation["tracking_correction_steps"] == 2 + + +def test_executor_holds_waypoint_until_bounded_tracking_converges(): + env = _Env() + adapter = _Adapter() + commanded_targets = [] + + def action_with_one_step_lag(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + target = target_eef_pose_dict["tool"] + commanded_targets.append(target.clone()) + adapter.pose[0, :3, 3] += 0.5 * (target[:3, 3] - adapter.pose[0, :3, 3]) + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_with_one_step_lag + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_eef_position_error_m=0.03, + max_tracking_correction_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success + assert len(env.step_calls) == 5 + assert result.final_observation["tracking_correction_steps"] == 1 + assert result.final_observation["terminal_correction_steps"] == 3 + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["target_segment_id"] == "move" + assert boundary["target_sample_index"] == 0 + assert boundary["initial_position_error_m"] == pytest.approx(0.025) + assert boundary["final_position_error_m"] == pytest.approx(0.003125, abs=1e-7) + assert boundary["correction_steps"] == 3 + assert boundary["gripper_unchanged"] is True + assert boundary["outcome"] == "converged" + assert boundary["max_correction_steps"] == 32 + assert boundary["convergence_trace"] == [ + { + "correction_steps": 0, + "joint_path_verified": True, + "position_error_m": pytest.approx(0.025, abs=1e-7), + "rotation_error_rad": pytest.approx(0.0), + }, + { + "correction_steps": 1, + "joint_path_verified": True, + "position_error_m": pytest.approx(0.0125, abs=1e-7), + "rotation_error_rad": pytest.approx(0.0), + }, + { + "correction_steps": 2, + "joint_path_verified": True, + "position_error_m": pytest.approx(0.00625, abs=1e-7), + "rotation_error_rad": pytest.approx(0.0), + }, + { + "correction_steps": 3, + "joint_path_verified": True, + "position_error_m": pytest.approx(0.003125, abs=1e-7), + "rotation_error_rad": pytest.approx(0.0), + }, + ] + assert boundary["post_settle_verification"] == { + "gripper_unchanged": True, + "joint_path_verified": True, + "passed": True, + "position_error_m": pytest.approx(0.003125, abs=1e-7), + "rotation_error_rad": pytest.approx(0.0), + "verification_samples": 1, + } + assert all(torch.equal(target, torch.tensor(_identity(0.1))) for target in commanded_targets) + assert adapter.grippers == [1.0] * 5 + + +def test_terminal_target_nonconvergence_fails_before_verification(): + env = _Env() + adapter = _Adapter() + verifier_calls = [] + + def action_without_observation_update(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + del target_eef_pose_dict + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + def verifier(*_args): + verifier_calls.append(True) + return True + + adapter.target_eef_pose_to_action = action_without_observation_update + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.05),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + verifier, + final_settle_steps=0, + max_tracking_correction_steps=0, + max_interaction_correction_steps=0, + max_terminal_correction_steps=2, + ) + assert executor.max_interaction_correction_steps == 0 + assert executor.max_terminal_correction_steps == 2 + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_stage is FailureStage.EXECUTION + assert result.failure_code == "terminal_target_not_reached" + assert result.recoverable is True + assert verifier_calls == [] + assert adapter.grippers == [1.0, 1.0, 1.0] + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["correction_steps"] == 2 + assert boundary["final_position_error_m"] == pytest.approx(0.05) + assert boundary["outcome"] == "failed_tolerance_not_reached" + assert boundary["max_correction_steps"] == 2 + assert boundary["convergence_trace"] == [ + { + "correction_steps": step, + "joint_path_verified": True, + "position_error_m": pytest.approx(0.05), + "rotation_error_rad": pytest.approx(0.0), + } + for step in range(3) + ] + + +@pytest.mark.parametrize("max_terminal_correction_steps", [True, -1, 101]) +def test_executor_rejects_unbounded_terminal_correction_budget(max_terminal_correction_steps): + with pytest.raises(ValueError, match="max_terminal_correction_steps must be an integer in \\[0, 100\\]"): + IsaacLabPlanExecutor( + _Env(), + _Adapter(), + lambda *_: True, + max_terminal_correction_steps=max_terminal_correction_steps, + ) + + +def test_terminal_trace_serializes_nonfinite_tracking_measurements_as_null(): + class _NonfiniteTerminalAdapter(_Adapter): + def __init__(self): + super().__init__() + self.pose_reads = 0 + + def get_eef_poses(self, env_ids): + result = super().get_eef_poses(env_ids) + self.pose_reads += 1 + if 3 <= self.pose_reads <= 7: + result["tool"][0, 0, 3] = float("nan") + return result + + env = _Env() + adapter = _NonfiniteTerminalAdapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_tracking_correction_steps=0, + max_terminal_correction_steps=1, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_code == "terminal_target_not_reached" + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["initial_position_error_m"] is None + assert boundary["final_position_error_m"] is None + assert [sample["position_error_m"] for sample in boundary["convergence_trace"]] == [None, None] + assert json.loads(json.dumps(boundary, allow_nan=False)) == boundary + + +def test_post_settle_cartesian_drift_fails_even_when_task_verifier_stays_true(): + class _PostSettleDriftEnv(_Env): + def __init__(self, adapter): + super().__init__() + self.adapter = adapter + + def step(self, action): + super().step(action) + if len(self.step_calls) == 2: + self.adapter.pose[0, 0, 3] += 0.01 + + adapter = _Adapter() + env = _PostSettleDriftEnv(adapter) + verifier_calls = [] + + def verifier(*_args): + verifier_calls.append(True) + return True + + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, verifier, final_settle_steps=1) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "terminal_target_not_stable" + assert result.recoverable is True + assert verifier_calls == [True, True] + post_settle = result.final_observation["terminal_target_boundaries"][0]["post_settle_verification"] + assert post_settle == { + "gripper_unchanged": True, + "joint_path_verified": True, + "passed": False, + "position_error_m": pytest.approx(0.01), + "rotation_error_rad": pytest.approx(0.0), + "verification_samples": 2, + } + + +def test_post_settle_joint_drift_fails_terminal_stability_gate(): + env = _Env() + adapter = _Adapter() + joint_reads = 0 + + def joint_positions(env_ids): + nonlocal joint_reads + del env_ids + joint_reads += 1 + return torch.tensor([[0.1, 0.2]]) if joint_reads < 3 else torch.tensor([[0.6, 0.2]]) + + adapter.get_joint_positions = joint_positions + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + joint_seed_names=("j1", "j2"), + joint_seeds=((0.1, 0.2),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=1) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "terminal_target_not_stable" + assert result.final_observation["maximum_joint_path_error_name"] == "j1" + assert result.final_observation["maximum_joint_path_error_rad"] == pytest.approx(0.5) + post_settle = result.final_observation["terminal_target_boundaries"][0]["post_settle_verification"] + assert post_settle["position_error_m"] == pytest.approx(0.0) + assert post_settle["joint_path_verified"] is False + assert post_settle["passed"] is False + + +def test_post_settle_nonfinite_tracking_evidence_is_json_safe_and_fails_closed(): + class _NonfinitePostSettleAdapter(_Adapter): + def __init__(self): + super().__init__() + self.pose_reads = 0 + + def get_eef_poses(self, env_ids): + result = super().get_eef_poses(env_ids) + self.pose_reads += 1 + if self.pose_reads in (7, 8): + result["tool"][0, 0, 3] = float("nan") + return result + + env = _Env() + adapter = _NonfinitePostSettleAdapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=1) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "terminal_target_not_stable" + post_settle = result.final_observation["terminal_target_boundaries"][0]["post_settle_verification"] + assert post_settle["position_error_m"] is None + assert post_settle["joint_path_verified"] is False + assert post_settle["passed"] is False + assert json.loads(json.dumps(post_settle, allow_nan=False)) == post_settle + + +def test_terminal_target_does_not_relax_joint_path_corridor(): + env = _Env() + adapter = _Adapter() + joint_reads = 0 + + def joint_positions(env_ids): + nonlocal joint_reads + del env_ids + joint_reads += 1 + return torch.tensor([[0.1, 0.2]]) if joint_reads == 1 else torch.tensor([[0.6, 0.2]]) + + adapter.get_joint_positions = joint_positions + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + joint_seed_names=("j1", "j2"), + joint_seeds=((0.1, 0.2),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_tracking_correction_steps=0, + max_interaction_correction_steps=0, + max_terminal_correction_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_code == "terminal_target_not_reached" + assert result.final_observation["maximum_joint_path_error_name"] == "j1" + assert result.final_observation["maximum_joint_path_error_rad"] == pytest.approx(0.5) + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["initial_joint_path_verified"] is False + assert boundary["final_joint_path_verified"] is False + assert boundary["gripper_unchanged"] is True + assert [sample["joint_path_verified"] for sample in boundary["convergence_trace"]] == [False, False, False] + + +def test_terminal_target_boundary_is_a_clean_noop_without_cartesian_targets(): + env = _Env() + adapter = _Adapter() + segment = GripperCommandSegment( + segment_id="initial_open", + eef_name="tool", + command=GripperCommandMode.OPEN, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is True + assert result.final_observation["terminal_target_boundaries"] == [] + assert result.final_observation["terminal_correction_steps"] == 0 + assert len(env.step_calls) == 1 + + +def test_terminal_correction_budget_is_prevalidated_before_any_action(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_steps=2, + max_tracking_correction_steps=0, + max_interaction_correction_steps=0, + max_terminal_correction_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success is False + assert result.failure_code == "execution_step_limit" + assert "approximately 3 simulator steps" in result.failure_message + assert env.step_calls == [] + + +def test_executor_enforces_finite_joint_path_bound_by_default_when_diagnostics_exist(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + joint_seed_names=("j1", "j2"), + joint_seeds=((2.0, 0.2),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_tracking_correction_steps=0, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "joint_path_tracking_error" + assert result.final_observation["maximum_joint_path_error_name"] == "j1" + assert result.final_observation["maximum_joint_path_error_rad"] == pytest.approx(1.9) + + +def test_executor_cartesian_plan_without_joint_diagnostics_remains_supported(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.05),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert result.success + assert len(env.step_calls) == 1 + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["correction_steps"] == 0 + assert boundary["outcome"] == "already_converged" + + +def test_executor_rejects_disabling_finite_joint_path_bound(): + with pytest.raises(ValueError, match="must be finite and positive"): + IsaacLabPlanExecutor(_Env(), _Adapter(), lambda *_: True, max_joint_path_error_rad=None) + + +def test_executor_rejects_saturated_dik_pose_action_before_env_step(): + env = _Env() + adapter = _Adapter() + + def saturated_action(*_args, **_kwargs): + return torch.tensor([1.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0]) + + adapter.target_eef_pose_to_action = saturated_action + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "dik_pose_action_saturated" + assert result.final_observation["steps"] == 0 + assert env.step_calls == [] + + +def test_executor_rejects_nonfinite_dik_action_before_env_step(): + env = _Env() + adapter = _Adapter() + adapter.target_eef_pose_to_action = lambda *_args, **_kwargs: torch.tensor( + [float("nan"), 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + ) + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "raw_dik_action_invalid" + assert env.step_calls == [] + + +def test_executor_rejects_cartesian_translation_discontinuity_before_any_action(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.11),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "cartesian_translation_discontinuity" + assert env.step_calls == [] + + +def test_executor_rejects_cartesian_linear_velocity_before_any_action(): + env = _Env() + env.step_dt = 0.01 + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.05),), + metadata={"step_dt_s": 0.01}, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "cartesian_linear_velocity_limit" + assert env.step_calls == [] + + +def test_executor_rejects_cartesian_rotation_discontinuity_before_any_action(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_yaw(0.26),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "cartesian_rotation_discontinuity" + assert env.step_calls == [] + + +def test_executor_rejects_cartesian_angular_velocity_before_any_action(): + env = _Env() + env.step_dt = 0.01 + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_yaw(0.05),), + metadata={"step_dt_s": 0.01}, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "cartesian_angular_velocity_limit" + assert env.step_calls == [] + + +def test_executor_rejects_planner_control_dt_mismatch_before_any_action(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + metadata={"step_dt_s": 0.02}, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "cartesian_step_dt_mismatch" + assert env.step_calls == [] + + +def test_executor_rejects_target_outside_base_relative_franka_workspace(): + env = _Env() + adapter = _Adapter() + adapter.pose[0] = torch.tensor(_identity(1.01)) + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(1.01),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "eef_workspace_limit" + assert env.step_calls == [] + + +def test_executor_fails_closed_when_workspace_frame_is_unavailable(): + env = _Env() + env.scene.articulations = {"robot": object()} + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "workspace_frame_unavailable" + assert env.step_calls == [] + + +def test_executor_enforces_explicit_joint_path_bound_when_configured(): + env = _Env() + adapter = _Adapter() + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + joint_seed_names=("j1", "j2"), + joint_seeds=((2.0, 0.2),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_joint_path_error_rad=0.1, + max_tracking_correction_steps=1, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "joint_path_tracking_error" + assert len(env.step_calls) == 2 + + +def test_gripper_transition_corrects_cartesian_lag_before_close_with_old_gripper(): + env = _Env() + adapter = _Adapter() + + def action_with_half_step_lag(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + target = target_eef_pose_dict["tool"] + adapter.pose[0, :3, 3] += 0.5 * (target[:3, 3] - adapter.pose[0, :3, 3]) + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_with_half_step_lag + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_interaction_correction_steps=5, + ) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert result.success + assert adapter.grippers == [1.0, 1.0, 1.0, 1.0, 1.0, -1.0] + assert result.final_observation["interaction_correction_steps"] == 4 + boundary = result.final_observation["interaction_boundaries"][0] + assert boundary["outcome"] == "converged" + assert boundary["correction_steps"] == 4 + assert boundary["initial_position_error_m"] == pytest.approx(0.05) + assert boundary["final_position_error_m"] == pytest.approx(0.003125, abs=1e-7) + assert boundary["target_segment_id"] == "move" + assert boundary["target_sample_index"] == 0 + assert result.final_observation["gripper_milestones"][0]["interaction_boundary"] == boundary + + +def test_gripper_transition_corrects_rotation_to_strict_interaction_tolerance(): + env = _Env() + adapter = _Adapter() + + def action_with_rotation_lag(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + target = target_eef_pose_dict["tool"] + current_yaw = torch.atan2(adapter.pose[0, 1, 0], adapter.pose[0, 0, 0]) + target_yaw = torch.atan2(target[1, 0], target[0, 0]) + next_yaw = float(current_yaw + 0.5 * (target_yaw - current_yaw)) + adapter.pose[0] = torch.tensor(_yaw(next_yaw)) + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_with_rotation_lag + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_yaw(0.12),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert result.success + boundary = result.final_observation["interaction_boundaries"][0] + assert boundary["correction_steps"] == 1 + assert boundary["initial_rotation_error_rad"] == pytest.approx(0.06, abs=1e-5) + assert boundary["final_rotation_error_rad"] == pytest.approx(0.03, abs=1e-5) + assert adapter.grippers == [1.0, 1.0, -1.0] + + +def test_gripper_transition_nonconvergence_fails_before_close_command(): + env = _Env() + adapter = _Adapter() + + def action_without_observation_update(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + del target_eef_pose_dict + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_without_observation_update + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.05),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_interaction_correction_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert not result.success + assert result.failure_code == "interaction_target_not_reached" + assert adapter.grippers == [1.0, 1.0, 1.0] + assert len(env.step_calls) == 3 + assert result.final_observation["gripper_milestones"] == [] + boundary = result.final_observation["interaction_boundaries"][0] + assert boundary["converged"] is False + assert boundary["correction_steps"] == 2 + assert boundary["outcome"] == "failed_tolerance_not_reached" + assert result.final_observation["interaction_correction_steps"] == 2 + + +def test_close_interaction_does_not_use_release_contact_tolerance() -> None: + env = _Env() + adapter = _Adapter() + + def action_stalled_nine_millimeters_from_target(target_eef_pose_dict, gripper_action_dict, *_args, **_kwargs): + target = target_eef_pose_dict["tool"].clone() + target[0, 3] -= 0.0095 + adapter.pose[0] = target + adapter.grippers.append(float(gripper_action_dict["tool"][0])) + return torch.zeros(7) + + adapter.target_eef_pose_to_action = action_stalled_nine_millimeters_from_target + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(0.1),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert result.success is False + assert result.failure_code == "interaction_target_not_reached" + assert all(command > 0 for command in adapter.grippers) + boundary = result.final_observation["interaction_boundaries"][0] + assert boundary["strict_target_converged"] is False + assert boundary["release_contact_gate_required"] is False + assert boundary["contact_constrained_task_success_accepted"] is False + + +def test_interaction_gate_preserves_joint_seed_corridor_before_close(): + env = _Env() + adapter = _Adapter() + joint_reads = 0 + + def joint_positions(env_ids): + nonlocal joint_reads + del env_ids + joint_reads += 1 + return torch.tensor([[0.1, 0.2]]) if joint_reads == 1 else torch.tensor([[1.0, 0.2]]) + + adapter.get_joint_positions = joint_positions + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + joint_seed_names=("j1", "j2"), + joint_seeds=((0.1, 0.2),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_interaction_correction_steps=1, + ) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert not result.success + assert result.failure_code == "interaction_target_not_reached" + assert adapter.grippers == [1.0, 1.0] + assert result.final_observation["maximum_joint_path_error_name"] == "j1" + assert result.final_observation["maximum_joint_path_error_rad"] == pytest.approx(0.9) + assert result.final_observation["interaction_boundaries"][0]["converged"] is False + + +def test_initial_open_records_no_preceding_interaction_target(): + env = _Env() + adapter = _Adapter() + initial_open = GripperCommandSegment( + segment_id="initial_open", + eef_name="tool", + command=GripperCommandMode.OPEN, + ) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=0) + + result = asyncio.run(executor.execute(_request(), _plan(initial_open))) + + assert result.success + boundary = result.final_observation["interaction_boundaries"][0] + assert boundary["preceding_cartesian_target"] is False + assert boundary["transition_required"] is False + assert boundary["outcome"] == "not_required_no_preceding_target" + assert result.final_observation["gripper_milestones"][0]["interaction_boundary"] == boundary + + +def test_interaction_correction_budget_is_prevalidated_before_any_action(): + env = _Env() + adapter = _Adapter() + move = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + close = GripperCommandSegment( + segment_id="close", + depends_on=("move",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: True, + final_settle_steps=0, + max_steps=2, + max_tracking_correction_steps=0, + max_interaction_correction_steps=2, + max_terminal_correction_steps=0, + ) + + result = asyncio.run(executor.execute(_request(), _plan(move, close))) + + assert not result.success + assert result.failure_code == "execution_step_limit" + assert env.step_calls == [] + + +def test_final_goal_must_hold_for_consecutive_stability_window(): + env = _Env() + adapter = _Adapter() + outcomes = iter((True, False, True)) + segment = CartesianTrajectorySegment( + segment_id="move", + eef_name="tool", + frame="env_origin", + poses=(_identity(),), + ) + executor = IsaacLabPlanExecutor( + env, + adapter, + lambda *_: next(outcomes), + final_settle_steps=2, + final_stability_steps=2, + ) + + result = asyncio.run(executor.execute(_request(), _plan(segment))) + + assert not result.success + assert result.failure_code == "final_goal_not_satisfied" + assert result.final_observation["final_success_streak"] == 1 + assert result.final_observation["verification_samples"] == 3 + + +class _SemanticAdapter(_Adapter): + def get_joint_positions(self, env_ids): + del env_ids + aperture = 0.08 if not self.grippers or self.grippers[-1] > 0 else 0.056 + return torch.tensor([[aperture / 2, aperture / 2]]) + + def get_joint_names(self): + return ["panda_finger_joint1", "panda_finger_joint2"] + + +class _SemanticEnv(_Env): + def __init__(self, adapter: _SemanticAdapter): + super().__init__() + self.adapter = adapter + self.closed_once = False + origin = self.scene.env_origins[0] + cube = self.scene.rigid_objects["cube"] + cube.data.root_pos_w = origin.unsqueeze(0).clone() + cube.data.root_lin_vel_w = torch.zeros((1, 3)) + cube.data.root_ang_vel_w = torch.zeros((1, 3)) + bowl_data = SimpleNamespace( + root_pos_w=(origin + torch.tensor([0.1, 0.0, 0.03])).unsqueeze(0), + root_quat_w=torch.tensor([[0.0, 0.0, 0.0, 1.0]]), + root_lin_vel_w=torch.zeros((1, 3)), + root_ang_vel_w=torch.zeros((1, 3)), + ) + bowl_cfg = SimpleNamespace(spawn=SimpleNamespace(usd_path="bowl.usd")) + self.scene.rigid_objects["bowl"] = SimpleNamespace(data=bowl_data, cfg=bowl_cfg) + + def step(self, action): + super().step(action) + gripper = self.adapter.grippers[-1] + cube = self.scene.rigid_objects["cube"] + if gripper < 0: + self.closed_once = True + cube_position = self.adapter.pose[0, :3, 3] + elif self.closed_once: + cube_position = torch.tensor([0.1, 0.0, 0.03]) + else: + cube_position = torch.zeros(3) + cube.data.root_pos_w[0] = self.scene.env_origins[0] + cube_position + + +def _task_placement_geometry(object_id, asset_name, dimensions): + return { + "aabb_center_in_converted_object_origin_m": [0.0, 0.0, 0.0], + "aabb_center_in_isaac_rigid_root_m": [0.0, 0.0, 0.0], + "aabb_dimensions_m": list(dimensions), + "aabb_kind": "converted_mesh_local_axis_aligned_bounding_box", + "asset_name": asset_name, + "converted_object_origin_from_aabb_center": [list(row) for row in _identity()], + "isaac_rigid_root_from_aabb_center": [list(row) for row in _identity()], + "object_id": object_id, + } + + +def _destination_placement_geometry(): + return { + "attested": True, + "destination": _task_placement_geometry("bowl", "bowl_ycb_robolab", (0.16, 0.16, 0.05)), + "frame_convention": "parent_from_child_homogeneous_4x4", + "general_inside_semantics": False, + "limitations": ["name_pinned_local_aabb_geometry_not_container_interior_geometry"], + "placement_model": "destination_local_aabb_top_plane_shifted_downward", + "predicted_aabb_center_offset_m": [0.0, 0.0, 0.03], + "profile": "franka_rubiks_cube_to_ycb_bowl_aabb_top_plane_v1", + "relation": "on", + "sampled_surface_extent_m": [0.0, 0.0, 0.0], + "schema_version": 1, + "subject": _task_placement_geometry( + "cube", + "rubiks_cube_hot3d_robolab", + (0.06, 0.06, 0.06), + ), + "surface_config": { + "implementation": "schedulestream.applications.custream.object.SurfaceConfig", + "xy_extend_m": -0.16, + "z_offset_m": -0.025, + }, + "vertical_evidence_corridor_m": 0.04, + } + + +def _grasp_geometry(): + primitives = [[list(row) for row in _yaw(angle)] for angle in (0.0, math.pi / 2, math.pi, 3 * math.pi / 2)] + return { + "asset_name": "rubiks_cube_hot3d_robolab", + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "converted_object_origin_from_aabb_center": [list(row) for row in _identity()], + "link_from_object_transforms": primitives, + "generator_storage": "reusable_finite_tuple", + "grasp_count": 4, + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": "cube", + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "primitive_link_from_aabb_center_transforms": primitives, + "profile": "franka_rubiks_cube_offcenter_cuboid_top_v1", + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + + +def _pick_place_plan() -> TaskMotionPlan: + transport_poses = tuple(_translation(0.01 * index, 0.0, 0.004 * index) for index in range(1, 11)) + segments = ( + GripperCommandSegment(segment_id="initial_open", eef_name="tool", command=GripperCommandMode.OPEN), + CartesianTrajectorySegment( + segment_id="approach", + depends_on=("initial_open",), + eef_name="tool", + frame="env_origin", + poses=(_translation(0.0, 0.0, 0.0),), + ), + GripperCommandSegment( + segment_id="close", + depends_on=("approach",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ), + AttachIntentSegment( + segment_id="attach", + depends_on=("close",), + eef_name="tool", + object_name="cube", + verifier="contact_and_relative_motion_v1", + ), + CartesianTrajectorySegment( + segment_id="transport", + depends_on=("attach",), + eef_name="tool", + frame="env_origin", + poses=transport_poses, + ), + GripperCommandSegment( + segment_id="release", + depends_on=("transport",), + eef_name="tool", + command=GripperCommandMode.OPEN, + ), + DetachIntentSegment( + segment_id="detach", + depends_on=("release",), + eef_name="tool", + object_name="cube", + verifier="contact_and_relative_motion_v1", + ), + CartesianTrajectorySegment( + segment_id="retreat", + depends_on=("detach",), + eef_name="tool", + frame="env_origin", + poses=(_translation(0.14, 0.0, 0.07), _translation(0.18, 0.0, 0.10)), + ), + ) + return TaskMotionPlan( + plan_id="pick_place_plan", + request_digest="request", + snapshot_digest="snapshot", + backend="schedulestream_custream", + backend_version="test", + seed=3, + segments=segments, + goal=(GoalPredicate("on", "cube", "bowl"),), + metadata={ + "arena_success_contract": {"attested": True}, + "schedulestream": { + "attachment_events_preserved": True, + "grasp_geometry": _grasp_geometry(), + "destination_placement": _destination_placement_geometry(), + }, + }, + ) + + +class _ReleaseContactAdapter(_SemanticAdapter): + def __init__(self, *, position_residual_m=0.0, rotation_residual_rad=0.0): + super().__init__() + self.position_residual_m = position_residual_m + self.rotation_residual_rad = rotation_residual_rad + + def target_eef_pose_to_action( + self, + target_eef_pose_dict, + gripper_action_dict, + action_noise_dict, + env_id, + ): + del action_noise_dict, env_id + target = target_eef_pose_dict["tool"].clone() + gripper = float(gripper_action_dict["tool"][0]) + if gripper < 0 and float(target[0, 3]) >= 0.099: + target[0, 3] -= self.position_residual_m + if self.rotation_residual_rad: + target[:3, :3] = torch.tensor(_yaw(self.rotation_residual_rad))[:3, :3] + self.pose[0] = target + self.grippers.append(gripper) + return torch.zeros(7) + + +class _OpenRetreatLagAdapter(_SemanticAdapter): + def target_eef_pose_to_action( + self, + target_eef_pose_dict, + gripper_action_dict, + action_noise_dict, + env_id, + ): + del action_noise_dict, env_id + target = target_eef_pose_dict["tool"] + gripper = float(gripper_action_dict["tool"][0]) + if gripper > 0 and any(value < 0 for value in self.grippers): + self.pose[0, :3, 3] += 0.25 * (target[:3, 3] - self.pose[0, :3, 3]) + self.pose[0, :3, :3] = target[:3, :3] + else: + self.pose[0] = target + self.grippers.append(gripper) + return torch.zeros(7) + + +class _DisplacedDestinationSemanticEnv(_SemanticEnv): + def step(self, action): + super().step(action) + if self.closed_once and self.adapter.grippers[-1] < 0 and float(self.adapter.pose[0, 0, 3]) >= 0.099: + origin = self.scene.env_origins[0] + self.scene.rigid_objects["bowl"].data.root_pos_w[0] = origin + torch.tensor([0.13, 0.0, 0.03]) + + +class _NonfiniteReleaseSampleAdapter(_SemanticAdapter): + def __init__(self): + super().__init__() + self.final_target_pose_reads = 0 + + def get_eef_poses(self, env_ids): + result = super().get_eef_poses(env_ids) + if float(self.pose[0, 0, 3]) >= 0.099: + self.final_target_pose_reads += 1 + if self.final_target_pose_reads == 5: + result["tool"][0, 0, 3] = float("nan") + return result + + +def _pick_place_plan_with_joint_seed() -> TaskMotionPlan: + plan = _pick_place_plan() + segments = list(plan.segments) + transport_index = next(index for index, segment in enumerate(segments) if segment.segment_id == "transport") + transport = segments[transport_index] + assert isinstance(transport, CartesianTrajectorySegment) + segments[transport_index] = replace( + transport, + joint_seed_names=("j1",), + joint_seeds=tuple((0.1,) for _ in transport.poses), + ) + return replace(plan, segments=tuple(segments)) + + +def test_schedulestream_executor_requires_and_records_complete_pick_place_success_report(): + adapter = _SemanticAdapter() + env = _SemanticEnv(adapter) + + def goal_verifier(_env, _env_id): + cube = env.scene.rigid_objects["cube"].data.root_pos_w[0] + bowl = env.scene.rigid_objects["bowl"].data.root_pos_w[0] + return torch.linalg.norm(cube - bowl).item() < 1e-5 + + executor = IsaacLabPlanExecutor(env, adapter, goal_verifier, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is True + report = result.final_observation["task_success_report"] + assert report["passed"] is True + assert report["closed_transport"]["samples"] == 10 + assert report["plan_lifecycle"]["attach_segment_id"] == "attach" + assert all(check["passed"] for check in report["checks"]) + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["strict_target_converged"] is True + assert release_boundary["contact_constrained_task_success_accepted"] is False + assert release_boundary["convergence_mode"] == "strict_task_success_ready" + assert release_boundary["task_success_release_gate"]["passed"] is True + + +def test_post_settle_terminal_drift_overrides_successful_task_verification(): + class _PostSettleSemanticDriftEnv(_SemanticEnv): + def __init__(self, adapter): + super().__init__(adapter) + self.final_target_steps = 0 + + def step(self, action): + super().step(action) + if self.adapter.grippers[-1] > 0 and float(self.adapter.pose[0, 0, 3]) >= 0.179: + self.final_target_steps += 1 + if self.final_target_steps == 2: + self.adapter.pose[0, 0, 3] += 0.01 + + adapter = _SemanticAdapter() + env = _PostSettleSemanticDriftEnv(adapter) + + def goal_verifier(_env, _env_id): + cube = env.scene.rigid_objects["cube"].data.root_pos_w[0] + bowl = env.scene.rigid_objects["bowl"].data.root_pos_w[0] + return torch.linalg.norm(cube - bowl).item() < 1e-5 + + executor = IsaacLabPlanExecutor(env, adapter, goal_verifier, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is False + assert result.failure_stage is FailureStage.VERIFICATION + assert result.failure_code == "terminal_target_not_stable" + assert result.final_observation["task_success_report"]["passed"] is True + post_settle = result.final_observation["terminal_target_boundaries"][0]["post_settle_verification"] + assert post_settle["position_error_m"] == pytest.approx(0.01) + assert post_settle["passed"] is False + + +def test_checked_release_and_lagging_retreat_converge_before_physical_success(): + adapter = _OpenRetreatLagAdapter() + env = _SemanticEnv(adapter) + + def goal_verifier(_env, _env_id): + cube = env.scene.rigid_objects["cube"].data.root_pos_w[0] + bowl = env.scene.rigid_objects["bowl"].data.root_pos_w[0] + return torch.linalg.norm(cube - bowl).item() < 1e-5 + + executor = IsaacLabPlanExecutor(env, adapter, goal_verifier, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is True + assert result.final_observation["task_success_report"]["passed"] is True + checks = {check["name"]: check for check in result.final_observation["task_success_report"]["checks"]} + assert checks["final_eef_subject_separation_m"]["passed"] is True + boundary = result.final_observation["terminal_target_boundaries"][0] + assert boundary["target_segment_id"] == "retreat" + assert boundary["target_sample_index"] == 1 + assert boundary["initial_position_error_m"] > 0.06 + assert boundary["final_position_error_m"] <= 0.005 + assert 1 <= boundary["correction_steps"] <= 12 + assert boundary["gripper_value"] == 1.0 + assert boundary["gripper_unchanged"] is True + assert boundary["outcome"] == "converged" + + +@pytest.mark.parametrize("position_residual_m", [0.0095, 0.0200]) +def test_release_contact_gate_accepts_bounded_contact_residual_with_explicit_evidence(position_residual_m): + adapter = _ReleaseContactAdapter(position_residual_m=position_residual_m) + env = _SemanticEnv(adapter) + + def goal_verifier(_env, _env_id): + cube = env.scene.rigid_objects["cube"].data.root_pos_w[0] + bowl = env.scene.rigid_objects["bowl"].data.root_pos_w[0] + return torch.linalg.norm(cube - bowl).item() < 1e-5 + + executor = IsaacLabPlanExecutor(env, adapter, goal_verifier, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is True + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["strict_target_converged"] is False + assert release_boundary["contact_constrained_cartesian_ready"] is True + assert release_boundary["contact_constrained_task_success_accepted"] is True + assert release_boundary["convergence_mode"] == "contact_constrained_task_success_ready" + assert release_boundary["release_contact_position_tolerance_m"] == 0.02 + assert release_boundary["release_contact_rotation_tolerance_rad"] == 0.05 + assert release_boundary["final_position_error_m"] == pytest.approx(position_residual_m, abs=1e-6) + assert release_boundary["task_success_release_gate"]["passed"] is True + + +@pytest.mark.parametrize( + ("position_residual_m", "rotation_residual_rad"), + [(0.0201, 0.0), (0.0095, 0.051)], +) +def test_release_contact_gate_rejects_residual_outside_cartesian_contact_corridor( + position_residual_m, + rotation_residual_rad, +): + adapter = _ReleaseContactAdapter( + position_residual_m=position_residual_m, + rotation_residual_rad=rotation_residual_rad, + ) + env = _SemanticEnv(adapter) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is False + assert result.failure_code == "interaction_target_not_reached" + assert adapter.grippers[-1] < 0 + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["strict_target_converged"] is False + assert release_boundary["contact_constrained_cartesian_ready"] is False + assert release_boundary["contact_constrained_task_success_accepted"] is False + assert release_boundary["task_success_release_gate"]["passed"] is True + + +def test_release_contact_gate_blocks_invalid_destination_even_at_strict_eef_target() -> None: + adapter = _SemanticAdapter() + env = _DisplacedDestinationSemanticEnv(adapter) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is False + assert result.failure_code == "release_task_success_not_ready" + assert adapter.grippers[-1] < 0 + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["strict_target_converged"] is True + assert release_boundary["outcome"] == "failed_task_success_release_not_ready" + checks = {check["name"]: check for check in release_boundary["task_success_release_gate"]["checks"]} + assert checks["prerelease_subject_target_horizontal_radius_m"]["passed"] is False + assert checks["prerelease_maximum_destination_drift_m"]["passed"] is False + + +def test_release_contact_gate_does_not_relax_joint_path_corridor() -> None: + class _JointDriftContactAdapter(_ReleaseContactAdapter): + def __init__(self): + super().__init__(position_residual_m=0.0095) + self.closed_final_target_calls = 0 + self.joint_drifted = False + + def get_joint_names(self): + return ["j1", "panda_finger_joint1", "panda_finger_joint2"] + + def get_joint_positions(self, env_ids): + del env_ids + aperture = 0.08 if not self.grippers or self.grippers[-1] > 0 else 0.056 + j1 = 0.6 if self.joint_drifted else 0.1 + return torch.tensor([[j1, aperture / 2, aperture / 2]]) + + def target_eef_pose_to_action(self, target_eef_pose_dict, gripper_action_dict, action_noise_dict, env_id): + target = target_eef_pose_dict["tool"] + if float(gripper_action_dict["tool"][0]) < 0 and float(target[0, 3]) >= 0.099: + self.closed_final_target_calls += 1 + self.joint_drifted = self.closed_final_target_calls >= 2 + return super().target_eef_pose_to_action( + target_eef_pose_dict, + gripper_action_dict, + action_noise_dict, + env_id, + ) + + adapter = _JointDriftContactAdapter() + env = _SemanticEnv(adapter) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan_with_joint_seed())) + + assert result.success is False + assert result.failure_code == "interaction_target_not_reached" + assert adapter.grippers[-1] < 0 + assert result.final_observation["maximum_joint_path_error_name"] == "j1" + assert result.final_observation["maximum_joint_path_error_rad"] == pytest.approx(0.5) + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["contact_constrained_cartesian_ready"] is False + assert release_boundary["task_success_release_gate"]["passed"] is True + + +def test_release_contact_gate_fails_closed_on_nonfinite_current_physical_state() -> None: + adapter = _NonfiniteReleaseSampleAdapter() + env = _SemanticEnv(adapter) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: True, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is False + assert result.failure_code == "task_success_state_unavailable" + assert result.recoverable is False + assert adapter.grippers[-1] < 0 + release_boundary = next( + boundary + for boundary in result.final_observation["interaction_boundaries"] + if boundary["segment_id"] == "release" + ) + assert release_boundary["outcome"] == "failed_task_success_unavailable" + assert release_boundary["task_success_release_gate"] is None + + +def test_schedulestream_executor_rejects_proximity_only_empty_gripper_before_logical_attach(): + class _EmptyGripperAdapter(_SemanticAdapter): + def get_joint_positions(self, env_ids): + del env_ids + aperture = 0.08 if not self.grippers or self.grippers[-1] > 0 else 0.006 + return torch.tensor([[aperture / 2, aperture / 2]]) + + adapter = _EmptyGripperAdapter() + env = _SemanticEnv(adapter) + executor = IsaacLabPlanExecutor(env, adapter, lambda *_: False, final_settle_steps=4) + + result = asyncio.run(executor.execute(_request(), _pick_place_plan())) + + assert result.success is False + assert result.failure_stage is FailureStage.EXECUTION + assert result.failure_code == "grasp_not_verified" + assert result.recoverable is True + assert executor.attachment_state.held_by_eef == {"tool": None} + report = result.final_observation["task_success_report"] + checks = {check["name"]: check for check in report["checks"]} + assert checks["attach_observed"]["passed"] is False + assert checks["grasp_aperture_min_m"]["observed"] == pytest.approx(0.006) diff --git a/isaac_autodata_tests/interfaces/autonomous/test_pick_place_success.py b/isaac_autodata_tests/interfaces/autonomous/test_pick_place_success.py new file mode 100644 index 0000000..7c5f344 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_pick_place_success.py @@ -0,0 +1,501 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import copy +import math + +import pytest + +from isaac_autodata_core.autonomous.task_motion import ( + AttachIntentSegment, + CartesianTrajectorySegment, + DetachIntentSegment, + GoalPredicate, + GripperCommandMode, + GripperCommandSegment, + TaskMotionPlan, +) +from isaac_autodata_interfaces.autonomous.pick_place_success import PickPlaceSuccessTracker + +IDENTITY = ( + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.0, 0.0, 0.0, 1.0), +) + + +def _geometry_record(object_id, asset_name, dimensions, center_offset=(0.0, 0.0, 0.0)): + transform = [list(row) for row in IDENTITY] + for index, value in enumerate(center_offset): + transform[index][3] = value + return { + "aabb_center_in_converted_object_origin_m": list(center_offset), + "aabb_center_in_isaac_rigid_root_m": list(center_offset), + "aabb_dimensions_m": list(dimensions), + "aabb_kind": "converted_mesh_local_axis_aligned_bounding_box", + "asset_name": asset_name, + "converted_object_origin_from_aabb_center": transform, + "isaac_rigid_root_from_aabb_center": transform, + "object_id": object_id, + } + + +def _placement_geometry(*, subject_offset=(0.0, 0.0, 0.0), target_offset=(0.0, 0.0, 0.0)): + return { + "attested": True, + "destination": _geometry_record("bowl", "bowl_ycb_robolab", (0.16, 0.16, 0.05), target_offset), + "frame_convention": "parent_from_child_homogeneous_4x4", + "general_inside_semantics": False, + "limitations": ["name_pinned_local_aabb_geometry_not_container_interior_geometry"], + "placement_model": "destination_local_aabb_top_plane_shifted_downward", + "predicted_aabb_center_offset_m": [0.0, 0.0, 0.03], + "profile": "franka_rubiks_cube_to_ycb_bowl_aabb_top_plane_v1", + "relation": "on", + "sampled_surface_extent_m": [0.0, 0.0, 0.0], + "schema_version": 1, + "subject": _geometry_record( + "cube", + "rubiks_cube_hot3d_robolab", + (0.06, 0.06, 0.06), + subject_offset, + ), + "surface_config": { + "implementation": "schedulestream.applications.custream.object.SurfaceConfig", + "xy_extend_m": -0.16, + "z_offset_m": -0.025, + }, + "vertical_evidence_corridor_m": 0.04, + } + + +def _multiply4(left, right): + return [ + [sum(left[row][index] * right[index][column] for index in range(4)) for column in range(4)] for row in range(4) + ] + + +def _inverse_translation(transform): + result = [list(row) for row in IDENTITY] + for index in range(3): + result[index][3] = -transform[index][3] + return result + + +def _grasp_geometry(object_from_aabb): + primitives = [] + for yaw in (0.0, math.pi / 2, math.pi, 3 * math.pi / 2): + cosine = math.cos(yaw) + sine = math.sin(yaw) + primitives.append([ + [cosine, -sine, 0.0, 0.0], + [sine, cosine, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ]) + aabb_from_object = _inverse_translation(object_from_aabb) + return { + "asset_name": "rubiks_cube_hot3d_robolab", + "attested": True, + "composition_formula": "primitive_link_from_aabb_center*inverse(converted_object_origin_from_aabb_center)", + "converted_object_origin_from_aabb_center": object_from_aabb, + "link_from_object_transforms": [_multiply4(primitive, aabb_from_object) for primitive in primitives], + "generator_storage": "reusable_finite_tuple", + "grasp_count": 4, + "link_target_formula": ( + "world_from_object*converted_object_origin_from_aabb_center*inverse(primitive_link_from_aabb_center)" + ), + "object_id": "cube", + "pitch_interval": "top", + "pose_convention": "link_from_object_parent_from_child_homogeneous_4x4", + "primitive": "cuboid", + "primitive_link_from_aabb_center_transforms": primitives, + "profile": "franka_rubiks_cube_offcenter_cuboid_top_v1", + "schema_version": 1, + "source": "schedulestream.applications.custream.grasp.primitive_grasp_generator", + } + + +def _plan(*, include_detach: bool = True, destination_placement=None, grasp_geometry=None) -> TaskMotionPlan: + destination_placement = destination_placement or _placement_geometry() + grasp_geometry = grasp_geometry or _grasp_geometry( + destination_placement["subject"]["converted_object_origin_from_aabb_center"] + ) + segments = [ + GripperCommandSegment(segment_id="initial_open", eef_name="tool", command=GripperCommandMode.OPEN), + CartesianTrajectorySegment( + segment_id="approach", + depends_on=("initial_open",), + eef_name="tool", + frame="env_origin", + poses=(IDENTITY,), + ), + GripperCommandSegment( + segment_id="close", + depends_on=("approach",), + eef_name="tool", + command=GripperCommandMode.CLOSE, + ), + AttachIntentSegment( + segment_id="attach", + depends_on=("close",), + eef_name="tool", + object_name="cube", + verifier="contact_and_relative_motion_v1", + ), + CartesianTrajectorySegment( + segment_id="transport", + depends_on=("attach",), + eef_name="tool", + frame="env_origin", + poses=(IDENTITY,), + ), + GripperCommandSegment( + segment_id="release", + depends_on=("transport",), + eef_name="tool", + command=GripperCommandMode.OPEN, + ), + ] + if include_detach: + segments.append( + DetachIntentSegment( + segment_id="detach", + depends_on=("release",), + eef_name="tool", + object_name="cube", + verifier="contact_and_relative_motion_v1", + ) + ) + return TaskMotionPlan( + plan_id="plan", + request_digest="request", + snapshot_digest="snapshot", + backend="schedulestream_custream", + backend_version="test", + seed=3, + segments=tuple(segments), + goal=(GoalPredicate("on", "cube", "bowl"),), + metadata={ + "arena_success_contract": {"attested": True}, + "schedulestream": { + "attachment_events_preserved": True, + "grasp_geometry": grasp_geometry, + "destination_placement": destination_placement, + }, + }, + ) + + +def _sample( + *, + cube=(0.0, 0.0, 0.0), + bowl=(0.1, 0.0, 0.03), + eef=(0.0, 0.0, 0.0), + aperture=0.08, + linear_speed=0.0, + angular_speed=0.0, +): + eef_pose = [list(row) for row in IDENTITY] + for index, value in enumerate(eef): + eef_pose[index][3] = value + return { + "eef_poses_env": {"tool": eef_pose}, + "joint_positions": { + "panda_finger_joint1": aperture / 2, + "panda_finger_joint2": aperture / 2, + }, + "objects": { + "bowl": { + "angular_speed_rad_s": 0.0, + "linear_speed_m_s": 0.0, + "position_env_m": list(bowl), + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + }, + "cube": { + "angular_speed_rad_s": angular_speed, + "linear_speed_m_s": linear_speed, + "position_env_m": list(cube), + "quaternion_xyzw": [0.0, 0.0, 0.0, 1.0], + }, + }, + } + + +def _successful_tracker() -> PickPlaceSuccessTracker: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for index in range(1, 11): + position = (0.01 * index, 0.0, 0.004 * index) + tracker.observe_step(_sample(cube=position, eef=position, aperture=0.056)) + release = _sample(cube=(0.1, 0.0, 0.04), eef=(0.1, 0.0, 0.04), aperture=0.056) + tracker.begin_release("release", release) + tracker.detach("detach", "cube", "tool", release) + final = _sample( + cube=(0.1, 0.0, 0.03), + eef=(0.2, 0.0, 0.10), + aperture=0.08, + linear_speed=0.01, + angular_speed=0.1, + ) + for _ in range(5): + tracker.observe_final(final, goal_satisfied=True) + return tracker + + +def _release_ready_tracker() -> PickPlaceSuccessTracker: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for index in range(1, 11): + position = (0.01 * index, 0.0, 0.004 * index) + tracker.observe_step(_sample(cube=position, eef=position, aperture=0.056)) + return tracker + + +def test_pick_place_success_accepts_observed_grasp_transport_release_and_stable_goal() -> None: + report = _successful_tracker().report(logical_held_object=None) + + assert report["passed"] is True + assert report["closed_transport"]["samples"] == 10 + assert report["closed_transport"]["maximum_lift_m"] == pytest.approx(0.04) + assert all(check["passed"] for check in report["checks"]) + + +def test_release_contact_gate_accepts_finite_attested_placement_and_transport() -> None: + gate = _release_ready_tracker().release_contact_gate( + "release", + _sample(cube=(0.1, 0.0, 0.04), eef=(0.0905, 0.0, 0.04), aperture=0.056), + ) + + assert gate["passed"] is True + assert gate["current_physical_state_finite"] is True + placement = gate["current_prerelease_placement"] + assert placement["subject_target_horizontal_radius_m"] == pytest.approx(0.0) + assert placement["subject_target_vertical_offset_abs_m"] == pytest.approx(0.01) + assert placement["subject_eef_attachment_distance_m"] == pytest.approx(0.0095) + assert placement["grasp_aperture_m"] == pytest.approx(0.056) + assert all(check["passed"] for check in gate["checks"]) + + +def test_release_contact_gate_rejects_accumulated_destination_drift_even_when_current_placement_matches() -> None: + gate = _release_ready_tracker().release_contact_gate( + "release", + _sample( + cube=(0.121, 0.0, 0.03), + bowl=(0.121, 0.0, 0.03), + eef=(0.1, 0.0, 0.04), + aperture=0.056, + ), + ) + + checks = {check["name"]: check for check in gate["checks"]} + assert gate["passed"] is False + assert checks["prerelease_subject_target_horizontal_radius_m"]["passed"] is True + assert checks["prerelease_maximum_destination_drift_m"]["observed"] == pytest.approx(0.021) + assert checks["prerelease_maximum_destination_drift_m"]["passed"] is False + + +@pytest.mark.parametrize( + ("cube", "failed_check"), + [ + ((0.129, 0.0, 0.03), "prerelease_subject_target_horizontal_radius_m"), + ((0.1, 0.0, 0.071), "prerelease_subject_target_vertical_offset_abs_m"), + ], +) +def test_release_contact_gate_rejects_prerelease_placement_outside_aabb_center_corridor(cube, failed_check) -> None: + gate = _release_ready_tracker().release_contact_gate( + "release", + _sample(cube=cube, eef=(0.1, 0.0, 0.04), aperture=0.056), + ) + + checks = {check["name"]: check for check in gate["checks"]} + assert gate["passed"] is False + assert checks[failed_check]["passed"] is False + + +def test_release_contact_gate_rejects_insufficient_closed_transport_samples() -> None: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for index in range(1, 10): + position = (0.1 if index == 9 else 0.01 * index, 0.0, 0.04 if index == 9 else 0.004 * index) + tracker.observe_step(_sample(cube=position, eef=position, aperture=0.056)) + + gate = tracker.release_contact_gate( + "release", + _sample(cube=(0.1, 0.0, 0.04), eef=(0.1, 0.0, 0.04), aperture=0.056), + ) + + checks = {check["name"]: check for check in gate["checks"]} + assert gate["passed"] is False + assert checks["prerelease_closed_transport_samples"]["observed"] == 9 + assert checks["prerelease_closed_transport_samples"]["passed"] is False + + +def test_release_contact_gate_rejects_closed_transport_relative_drift() -> None: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for index in range(1, 10): + position = (0.01 * index, 0.0, 0.004 * index) + tracker.observe_step(_sample(cube=position, eef=position, aperture=0.056)) + tracker.observe_step(_sample(cube=(0.1, 0.0, 0.04), eef=(0.075, 0.0, 0.04), aperture=0.056)) + + gate = tracker.release_contact_gate( + "release", + _sample(cube=(0.1, 0.0, 0.04), eef=(0.075, 0.0, 0.04), aperture=0.056), + ) + + checks = {check["name"]: check for check in gate["checks"]} + assert gate["passed"] is False + relative = checks["prerelease_closed_transport_relative_translation_drift_m"] + assert relative["observed"] == pytest.approx(0.025) + assert relative["passed"] is False + + +@pytest.mark.parametrize( + ("eef", "aperture", "failed_check"), + [ + ((0.151, 0.0, 0.04), 0.056, "prerelease_subject_eef_attachment_distance_m"), + ((0.1, 0.0, 0.04), 0.080, "prerelease_grasp_aperture_max_m"), + ((0.1, 0.0, 0.04), 0.039, "prerelease_grasp_aperture_min_m"), + ], +) +def test_release_contact_gate_rejects_lost_prerelease_grasp(eef, aperture, failed_check) -> None: + gate = _release_ready_tracker().release_contact_gate( + "release", + _sample(cube=(0.1, 0.0, 0.04), eef=eef, aperture=aperture), + ) + + checks = {check["name"]: check for check in gate["checks"]} + assert gate["passed"] is False + assert checks[failed_check]["passed"] is False + + +def test_release_contact_gate_fails_closed_on_nonfinite_current_state() -> None: + sample = copy.deepcopy(_sample(cube=(0.1, 0.0, 0.04), eef=(0.1, 0.0, 0.04), aperture=0.056)) + sample["eef_poses_env"]["tool"][0][3] = float("nan") + + with pytest.raises(ValueError, match="must be finite"): + _release_ready_tracker().release_contact_gate("release", sample) + + +def test_pick_place_success_measures_final_placement_between_validated_aabb_centers() -> None: + placement = _placement_geometry(subject_offset=(0.03, 0.0, 0.0)) + tracker = PickPlaceSuccessTracker.from_plan(_plan(destination_placement=placement), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for index in range(1, 11): + position = (0.007 * index, 0.0, 0.004 * index) + tracker.observe_step(_sample(cube=position, eef=position, aperture=0.056)) + release = _sample(cube=(0.07, 0.0, 0.03), eef=(0.07, 0.0, 0.03), aperture=0.056) + tracker.begin_release("release", release) + tracker.detach("detach", "cube", "tool", release) + final = _sample(cube=(0.07, 0.0, 0.03), eef=(0.2, 0.0, 0.1)) + for _ in range(5): + tracker.observe_final(final, goal_satisfied=True) + + report = tracker.report(logical_held_object=None) + + checks = {check["name"]: check for check in report["checks"]} + assert checks["final_subject_target_horizontal_radius_m"]["observed"] == pytest.approx(0.0) + assert checks["final_subject_target_horizontal_radius_m"]["passed"] is True + + +def test_pick_place_success_rejects_proximity_only_attachment_with_closed_empty_aperture() -> None: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + missed_grasp = _sample(aperture=0.006) + + assert tracker.attachment_is_plausible(missed_grasp) is False + with pytest.raises(ValueError, match="plausible physical aperture/distance"): + tracker.attach("attach", "cube", "tool", missed_grasp) + + report = tracker.report(logical_held_object=None) + checks = {check["name"]: check for check in report["checks"]} + assert checks["attach_observed"]["passed"] is False + assert checks["grasp_aperture_min_m"]["observed"] == pytest.approx(0.006) + assert checks["grasp_aperture_min_m"]["passed"] is False + + +def test_pick_place_success_rejects_incomplete_plan_lifecycle_before_execution() -> None: + with pytest.raises(ValueError, match="exactly one attach and one detach"): + PickPlaceSuccessTracker.from_plan(_plan(include_detach=False), ("tool",)) + + +def test_pick_place_success_rejects_missing_grasp_geometry_before_execution() -> None: + with pytest.raises(ValueError, match="exact grasp geometry"): + PickPlaceSuccessTracker.from_plan(_plan(grasp_geometry={"attested": True}), ("tool",)) + + +def test_pick_place_success_rejects_tampered_grasp_transform_before_execution() -> None: + grasp_geometry = _grasp_geometry([list(row) for row in IDENTITY]) + grasp_geometry["link_from_object_transforms"][0][0][3] += 0.001 + + with pytest.raises(ValueError, match="violates its composition formula"): + PickPlaceSuccessTracker.from_plan(_plan(grasp_geometry=grasp_geometry), ("tool",)) + + +def test_pick_place_success_rejects_grasp_and_placement_aabb_frame_mismatch() -> None: + placement = _placement_geometry(subject_offset=(0.03, 0.0, 0.0)) + grasp_geometry = _grasp_geometry([list(row) for row in IDENTITY]) + + with pytest.raises(ValueError, match="grasp and placement AABB transforms do not match"): + PickPlaceSuccessTracker.from_plan( + _plan(destination_placement=placement, grasp_geometry=grasp_geometry), + ("tool",), + ) + + +def test_pick_place_success_rejects_stationary_cube_even_when_arena_goal_is_true() -> None: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + tracker.settle_initial("initial_open", _sample(), goal_satisfied=False) + tracker.attach("attach", "cube", "tool", _sample(aperture=0.056)) + for _ in range(10): + tracker.observe_step(_sample(aperture=0.056)) + tracker.begin_release("release", _sample(aperture=0.056)) + tracker.detach("detach", "cube", "tool", _sample(aperture=0.056)) + final = _sample(cube=(0.1, 0.0, 0.03), eef=(0.2, 0.0, 0.1)) + for _ in range(5): + tracker.observe_final(final, goal_satisfied=True) + + report = tracker.report(logical_held_object=None) + + failed = {check["name"] for check in report["checks"] if not check["passed"]} + assert "closed_transport_lift_m" in failed + assert "closed_transport_subject_displacement_m" in failed + assert report["passed"] is False + + +def test_opening_settle_motion_is_excluded_from_closed_relative_drift() -> None: + tracker = _successful_tracker() + before = tracker.report(logical_held_object=None)["closed_transport"] + + slipped = _sample(cube=(0.1, 0.0, -0.5), eef=(0.5, 0.0, 0.5)) + tracker.observe_step(slipped) + after = tracker.report(logical_held_object=None)["closed_transport"] + + assert after == before + + +def test_pick_place_success_fails_closed_on_nonfinite_live_observation() -> None: + tracker = PickPlaceSuccessTracker.from_plan(_plan(), ("tool",)) + assert tracker is not None + sample = copy.deepcopy(_sample()) + sample["objects"]["cube"]["linear_speed_m_s"] = float("nan") + + with pytest.raises(ValueError, match="must be finite"): + tracker.settle_initial("initial_open", sample, goal_satisfied=False) diff --git a/isaac_autodata_tests/interfaces/autonomous/test_runtime_support.py b/isaac_autodata_tests/interfaces/autonomous/test_runtime_support.py new file mode 100644 index 0000000..1d8d4e1 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_runtime_support.py @@ -0,0 +1,678 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import copy +import subprocess +import sys +from dataclasses import replace +from pathlib import Path + +import pytest + +from isaac_autodata_interfaces.autonomous import ( + CURRENT_RUNTIME_SUPPORT, + ArenaCompilationResult, + CompiledTaskRequest, + GenerationConfig, + GoalStage, + MotionBackend, + PlannerBackend, + PlannerConfig, + ResolvedOutputConfig, + RuntimeSupportError, + SpatialGoalConstraint, + canonical_json, + sha256_json, + validate_runtime_support, +) + + +def _linked_graph() -> dict: + return { + "env_name": "llm_gen_maple_table_PickAndPlaceTask", + "nodes": [ + {"id": "robot", "name": "franka_ik", "type": "embodiment", "params": {}}, + {"id": "table", "name": "maple_table_robolab", "type": "background", "params": {}}, + {"id": "pick_cube", "name": "rubiks_cube_hot3d_robolab", "type": "object", "params": {}}, + {"id": "destination_bowl", "name": "bowl_ycb_robolab", "type": "object", "params": {}}, + ], + "tasks": [{ + "id": "task_0_PickAndPlaceTask", + "kind": "PickAndPlaceTask", + "params": { + "background_scene": "table", + "destination_location": "destination_bowl", + "pick_up_object": "pick_cube", + }, + "description": "place", + "initial_state_spec_id": "state_initial", + "success_state_spec_id": "state_success", + }], + "state_specs": [ + { + "id": "state_initial", + "is_delta": False, + "spatial_constraints": [ + { + "id": "state_initial_0_is_anchor_table", + "kind": "is_anchor", + "subject": "table", + "params": {}, + }, + { + "id": "state_initial_1_pick_cube_on_table", + "kind": "on", + "subject": "pick_cube", + "reference": "table", + "params": {}, + }, + { + "id": "state_initial_2_destination_bowl_on_table", + "kind": "on", + "subject": "destination_bowl", + "reference": "table", + "params": {}, + }, + ], + "task_constraints": [], + }, + { + "id": "state_success", + "is_delta": True, + "spatial_constraints": [{ + "id": "state_success_pick_cube_on_destination_bowl", + "kind": "on", + "subject": "pick_cube", + "reference": "destination_bowl", + "params": {}, + }], + "task_constraints": [], + }, + ], + "cli_override_specs": [], + } + + +def _goal_stage( + *, + kind: str = "on", + subject: str = "pick_cube", + reference: str | None = "destination_bowl", + params: dict | None = None, +) -> GoalStage: + return GoalStage( + index=0, + task_id="task_0_PickAndPlaceTask", + task_kind="PickAndPlaceTask", + success_state_spec_id="state_success", + spatial_constraints=( + SpatialGoalConstraint( + id="state_success_pick_cube_on_destination_bowl", + kind=kind, + subject=subject, + reference=reference, + params_json=canonical_json({} if params is None else params), + ), + ), + ) + + +def _resolved_request( + tmp_path: Path, + *, + linked_graph: dict | None = None, + goal_stages: tuple[GoalStage, ...] | None = None, + num_envs: int = 1, +) -> CompiledTaskRequest: + linked = copy.deepcopy(_linked_graph() if linked_graph is None else linked_graph) + stages = (_goal_stage(),) if goal_stages is None else goal_stages + arena = ArenaCompilationResult( + initial_graph_json=canonical_json({"env_name": linked["env_name"]}), + linked_graph_json=canonical_json(linked), + compiler_trace=(), + graph_digest=sha256_json(linked), + goal_stages=stages, + ) + return CompiledTaskRequest( + schema_version=1, + compiler_version="test", + name="capability_test", + canonical_request_json=canonical_json({"name": "capability_test"}), + request_digest="a" * 64, + planner=PlannerConfig( + backend=PlannerBackend.SCHEDULESTREAM, + motion_backend=MotionBackend.CUROBO_V1, + collisions=True, + max_time_s=10.0, + batch_size=4, + interpolation_dt_s=0.02, + profile=False, + animate=False, + ), + generation=GenerationConfig( + successful_episodes=1, + seed=7, + num_envs=num_envs, + max_attempts=2, + ), + output=ResolvedOutputConfig( + dataset=tmp_path / "data.hdf5", + keep_failed=False, + run_log=tmp_path / "data.jsonl", + ), + arena=arena, + ) + + +def _codes(exc: pytest.ExceptionInfo[RuntimeSupportError]) -> set[tuple[str, str]]: + return {(issue.field_path, issue.code) for issue in exc.value.issues} + + +def test_valid_profile_is_deterministic_and_attestable(tmp_path: Path) -> None: + request = _resolved_request(tmp_path) + + first = validate_runtime_support( + request, + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + second = validate_runtime_support( + request, + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert first == second + assert first.profile == CURRENT_RUNTIME_SUPPORT + assert first.pick_up_object_id == "pick_cube" + assert first.destination_location_id == "destination_bowl" + assert first.background_scene_id == "table" + assert first.to_dict()["task"]["success_state_spec_id"] == "state_success" + assert len(first.digest) == 64 + assert first.canonical_json() == canonical_json(first.to_dict()) + + +def test_capability_module_is_import_free() -> None: + script = """ +import json +import sys +import isaac_autodata_interfaces.autonomous.runtime_support +blocked = ('isaaclab', 'isaaclab_arena', 'torch', 'curobo', 'schedulestream', 'omni') +loaded = sorted(name for name in sys.modules if name.split('.')[0] in blocked) +print(json.dumps(loaded)) +""" + + result = subprocess.run([sys.executable, "-c", script], check=False, capture_output=True, text=True) + + assert result.returncode == 0, result.stderr + assert result.stdout.strip() == "[]" + + +def test_v2_is_explicitly_schema_only_not_live(tmp_path: Path) -> None: + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path), + motion_backend="curobo_v2", + schedulestream_application="custream2", + ) + + assert _codes(exc) == {("$.runtime.selected_motion_backend", "live_curobo_v2_unsupported")} + assert "no reviewed live IsaacLab provider" in exc.value.issues[0].message + + +@pytest.mark.parametrize( + ("motion_backend", "application", "expected"), + [ + ("unknown", "custream", ("$.runtime.selected_motion_backend", "live_motion_backend_unsupported")), + ( + "curobo_v1", + "custream2", + ("$.runtime.schedulestream_application", "live_schedulestream_application_unsupported"), + ), + ], +) +def test_mixed_or_unknown_runtime_is_rejected( + tmp_path: Path, + motion_backend: str, + application: str, + expected: tuple[str, str], +) -> None: + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path), + motion_backend=motion_backend, + schedulestream_application=application, + ) + + assert expected in _codes(exc) + + +def test_parallel_generation_is_rejected_before_launch(tmp_path: Path) -> None: + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, num_envs=2), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert _codes(exc) == {("$.generation.num_envs", "live_num_envs_unsupported")} + + +@pytest.mark.parametrize( + ("section", "field", "value", "expected"), + [ + ("planner", "collisions", False, ("$.planner.collisions", "live_collisions_required")), + ("planner", "max_time_s", 60.1, ("$.planner.max_time_s", "live_planner_time_limit")), + ("planner", "batch_size", 1025, ("$.planner.batch_size", "live_batch_size_limit")), + ("planner", "profile", True, ("$.planner.profile", "live_profile_mode_unsupported")), + ("planner", "animate", True, ("$.planner.animate", "live_animation_unsupported")), + ( + "planner", + "interpolation_dt_s", + 0.01, + ("$.planner.interpolation_dt_s", "live_interpolation_dt_unsupported"), + ), + ( + "generation", + "successful_episodes", + 11, + ("$.generation.successful_episodes", "live_success_target_limit"), + ), + ("generation", "max_attempts", 6, ("$.generation.max_attempts", "live_attempt_limit")), + ], +) +def test_live_profile_rejects_resource_amplification_before_launch( + tmp_path: Path, + section: str, + field: str, + value: object, + expected: tuple[str, str], +) -> None: + request = _resolved_request(tmp_path) + config = getattr(request, section) + request = replace(request, **{section: replace(config, **{field: value})}) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + request, + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert expected in _codes(exc) + + +def test_live_profile_requires_durable_run_log_before_launch(tmp_path: Path) -> None: + request = _resolved_request(tmp_path) + request = replace(request, output=replace(request.output, run_log=None)) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + request, + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert _codes(exc) == {("$.output.run_log", "live_run_log_required")} + + +@pytest.mark.parametrize("embodiment_name", ["droid_abs_joint_pos", "g1_ik"]) +def test_only_franka_ik_embodiment_is_live(tmp_path: Path, embodiment_name: str) -> None: + linked = _linked_graph() + linked["nodes"][0]["name"] = embodiment_name + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ( + "$.arena.linked_graph.nodes[0].name", + "live_embodiment_unsupported", + ) in _codes(exc) + + +def test_live_franka_profile_rejects_embodiment_overrides(tmp_path: Path) -> None: + linked = _linked_graph() + linked["nodes"][0]["params"] = {"initial_joint_pose": [0.0] * 9} + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ( + "$.arena.linked_graph.nodes[0].params", + "live_embodiment_params_unsupported", + ) in _codes(exc) + + +def test_live_scene_rejects_unreviewed_distractor_nodes(tmp_path: Path) -> None: + linked = _linked_graph() + linked["nodes"].append({"id": "distractor", "name": "bowl_ycb_robolab", "type": "object", "params": {}}) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ("$.arena.linked_graph.nodes", "live_graph_node_count_unsupported") in _codes(exc) + + +def test_multiple_embodiments_are_rejected(tmp_path: Path) -> None: + linked = _linked_graph() + linked["nodes"].append({"id": "robot_2", "name": "franka_ik", "type": "embodiment", "params": {}}) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ("$.arena.linked_graph.nodes", "live_embodiment_count_unsupported") in _codes(exc) + + +@pytest.mark.parametrize( + ("mutation", "expected"), + [ + ("second_task", ("$.arena.linked_graph.tasks", "live_task_count_unsupported")), + ("wrong_kind", ("$.arena.linked_graph.tasks[0].kind", "live_task_kind_unsupported")), + ], +) +def test_only_one_pick_and_place_task_is_live( + tmp_path: Path, + mutation: str, + expected: tuple[str, str], +) -> None: + linked = _linked_graph() + if mutation == "second_task": + linked["tasks"].append(copy.deepcopy(linked["tasks"][0])) + linked["tasks"][1]["id"] = "task_1" + else: + linked["tasks"][0]["kind"] = "OpenDoorTask" + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert expected in _codes(exc) + + +def test_task_params_are_exact_and_resolve_to_expected_node_types(tmp_path: Path) -> None: + linked = _linked_graph() + params = linked["tasks"][0]["params"] + params.pop("background_scene") + params["episode_length_s"] = 20.0 + params["destination_location"] = "table" + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + codes = _codes(exc) + assert ("$.arena.linked_graph.tasks[0].params.background_scene", "task_param_missing") in codes + assert ("$.arena.linked_graph.tasks[0].params.episode_length_s", "live_task_param_unsupported") in codes + assert ( + "$.arena.linked_graph.tasks[0].params.destination_location", + "task_param_node_type_unsupported", + ) in codes + + +@pytest.mark.parametrize( + ("asset_name", "asset_params"), + [ + ("bowl_ycb_robolab", {}), + ("rubiks_cube_hot3d_robolab", {"scale": 2.0}), + ], +) +def test_live_pickup_requires_reviewed_unmodified_cube_geometry( + tmp_path: Path, + asset_name: str, + asset_params: dict, +) -> None: + linked = _linked_graph() + linked["nodes"][2]["name"] = asset_name + linked["nodes"][2]["params"] = asset_params + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ( + "$.arena.linked_graph.tasks[0].params.pick_up_object", + "live_pick_up_geometry_unsupported", + ) in _codes(exc) + + +@pytest.mark.parametrize( + ("node_index", "field", "asset_name", "asset_params", "expected_code"), + [ + (1, "background_scene", "other_table", {}, "live_background_asset_unsupported"), + (1, "background_scene", "maple_table_robolab", {"scale": 2.0}, "live_background_asset_unsupported"), + (3, "destination_location", "other_bowl", {}, "live_destination_asset_unsupported"), + (3, "destination_location", "bowl_ycb_robolab", {"scale": 2.0}, "live_destination_asset_unsupported"), + ], +) +def test_live_scene_requires_reviewed_unmodified_table_and_bowl( + tmp_path: Path, + node_index: int, + field: str, + asset_name: str, + asset_params: dict, + expected_code: str, +) -> None: + linked = _linked_graph() + linked["nodes"][node_index]["name"] = asset_name + linked["nodes"][node_index]["params"] = asset_params + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert (f"$.arena.linked_graph.tasks[0].params.{field}", expected_code) in _codes(exc) + + +def test_task_and_state_graph_ids_must_be_unique_and_resolved(tmp_path: Path) -> None: + linked = _linked_graph() + linked["nodes"][3]["id"] = "pick_cube" + linked["tasks"][0]["success_state_spec_id"] = "missing_state" + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + codes = _codes(exc) + assert ("$.arena.linked_graph.nodes[3].id", "graph_id_duplicate") in codes + assert ("$.arena.linked_graph.tasks[0].success_state_spec_id", "state_spec_reference_missing") in codes + + +@pytest.mark.parametrize( + "unsafe_id", + [ + "../Robot", + "cube/name", + "{ENV_REGEX_NS}", + "cube.*", + "cube[0]", + "cube\nother", + "-cube", + "éclair", + "a" * 129, + ], +) +def test_live_object_ids_must_be_ascii_usd_safe(tmp_path: Path, unsafe_id: str) -> None: + linked = _linked_graph() + linked["nodes"][2]["id"] = unsafe_id + linked["tasks"][0]["params"]["pick_up_object"] = unsafe_id + linked["state_specs"][0]["spatial_constraints"][1]["subject"] = unsafe_id + linked["state_specs"][1]["spatial_constraints"][0]["subject"] = unsafe_id + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request( + tmp_path, + linked_graph=linked, + goal_stages=(_goal_stage(subject=unsafe_id),), + ), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ("$.arena.linked_graph.nodes[2].id", "graph_id_invalid") in _codes(exc) + + +@pytest.mark.parametrize( + ("mutation", "expected_code"), + [ + ("already_successful", "live_initial_state_semantics_unsupported"), + ("missing_constraints", "live_initial_state_semantics_unsupported"), + ("delta", "live_initial_state_delta_unsupported"), + ("task_constraint", "live_initial_task_constraints_unsupported"), + ("constraint_params", "live_initial_constraint_params_unsupported"), + ], +) +def test_live_initial_state_must_be_exact_and_nontrivial( + tmp_path: Path, + mutation: str, + expected_code: str, +) -> None: + linked = _linked_graph() + initial = linked["state_specs"][0] + if mutation == "already_successful": + initial["spatial_constraints"][1]["reference"] = "destination_bowl" + elif mutation == "missing_constraints": + initial["spatial_constraints"] = [] + elif mutation == "delta": + initial["is_delta"] = True + elif mutation == "task_constraint": + initial["task_constraints"] = [{"kind": "already_done"}] + else: + initial["spatial_constraints"][1]["params"] = {"margin": 0.01} + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert any(code == expected_code for _, code in _codes(exc)) + + +def test_goal_stage_must_match_the_single_linked_task(tmp_path: Path) -> None: + stage = replace(_goal_stage(), task_id="different_task", success_state_spec_id="different_state") + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, goal_stages=(stage,)), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + codes = _codes(exc) + assert ("$.arena.goal_stages[0].task_id", "goal_stage_task_mismatch") in codes + assert ("$.arena.goal_stages[0].success_state_spec_id", "goal_stage_task_mismatch") in codes + + +def test_goal_projection_must_exactly_match_linked_success_state(tmp_path: Path) -> None: + linked = _linked_graph() + linked["state_specs"][1]["spatial_constraints"][0]["reference"] = "table" + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, linked_graph=linked), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ( + "$.arena.goal_stages[0].spatial_constraints[0]", + "goal_projection_mismatch", + ) in _codes(exc) + + +def test_multiple_goal_stages_are_not_flattened(tmp_path: Path) -> None: + stages = (_goal_stage(), replace(_goal_stage(), index=1, task_id="task_1")) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, goal_stages=stages), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ("$.arena.goal_stages", "live_goal_stage_count_unsupported") in _codes(exc) + + +@pytest.mark.parametrize( + ("stage", "expected"), + [ + ( + _goal_stage(kind="in"), + ("$.arena.goal_stages[0].spatial_constraints[0].kind", "live_goal_relation_unsupported"), + ), + ( + _goal_stage(params={"margin": 0.01}), + ("$.arena.goal_stages[0].spatial_constraints[0].params", "live_goal_params_unsupported"), + ), + ( + _goal_stage(subject="destination_bowl", reference="pick_cube"), + ("$.arena.goal_stages[0].spatial_constraints[0].subject", "goal_constraint_task_mismatch"), + ), + ], +) +def test_goal_relation_params_and_task_endpoints_are_not_reinterpreted( + tmp_path: Path, + stage: GoalStage, + expected: tuple[str, str], +) -> None: + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, goal_stages=(stage,)), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert expected in _codes(exc) + + +def test_exactly_one_success_constraint_is_required(tmp_path: Path) -> None: + stage = replace(_goal_stage(), spatial_constraints=()) + + with pytest.raises(RuntimeSupportError) as exc: + validate_runtime_support( + _resolved_request(tmp_path, goal_stages=(stage,)), + motion_backend="curobo_v1", + schedulestream_application="custream", + ) + + assert ( + "$.arena.goal_stages[0].spatial_constraints", + "live_goal_constraint_count_unsupported", + ) in _codes(exc) diff --git a/isaac_autodata_tests/interfaces/autonomous/test_task_profile.py b/isaac_autodata_tests/interfaces/autonomous/test_task_profile.py new file mode 100644 index 0000000..01a5007 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_task_profile.py @@ -0,0 +1,39 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from dataclasses import FrozenInstanceError + +import pytest + +from isaac_autodata_interfaces.autonomous.pick_place_success import PickPlaceSuccessThresholds as VerifierThresholds +from isaac_autodata_interfaces.autonomous.profiles.franka_pick_cube_into_bowl import ( + FRANKA_PICK_CUBE_INTO_BOWL, + PickPlaceSuccessThresholds, +) +from isaac_autodata_interfaces.autonomous.runtime_support import CURRENT_RUNTIME_SUPPORT +from isaac_autodata_interfaces.autonomous.schedulestream.custream_v1 import ( + V1_DESTINATION_PLACEMENT_PROFILE, + V1_FRANKA_USD_BASENAMES, + V1_GRASP_GEOMETRY_PROFILE, + V1_REVIEWED_DESTINATION_ASSET, + V1_REVIEWED_GRASPABLE_ASSET, +) + + +def test_live_task_profile_is_the_single_owner_of_shared_runtime_facts() -> None: + profile = FRANKA_PICK_CUBE_INTO_BOWL + + assert CURRENT_RUNTIME_SUPPORT == profile.name + assert V1_FRANKA_USD_BASENAMES is profile.franka_usd_basenames + assert V1_REVIEWED_GRASPABLE_ASSET == profile.graspable_asset + assert V1_REVIEWED_DESTINATION_ASSET == profile.destination_asset + assert V1_GRASP_GEOMETRY_PROFILE == profile.grasp_geometry_profile + assert V1_DESTINATION_PLACEMENT_PROFILE == profile.destination_placement_profile + assert VerifierThresholds is PickPlaceSuccessThresholds + + +def test_live_task_profile_is_immutable() -> None: + with pytest.raises(FrozenInstanceError): + FRANKA_PICK_CUBE_INTO_BOWL.motion_backend = "other" diff --git a/isaac_autodata_tests/interfaces/autonomous/test_task_request.py b/isaac_autodata_tests/interfaces/autonomous/test_task_request.py new file mode 100644 index 0000000..8c6e336 --- /dev/null +++ b/isaac_autodata_tests/interfaces/autonomous/test_task_request.py @@ -0,0 +1,351 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import copy +import subprocess +import sys +from pathlib import Path + +import pytest + +from isaac_autodata_interfaces.autonomous import ( + AutonomousValidationError, + MotionBackend, + PlannerBackend, + load_task_request, + task_request_from_dict, +) +from isaac_autodata_interfaces.autonomous._yaml import MAX_YAML_BYTES + + +def _request_dict() -> dict: + return { + "schema_version": 1, + "name": "franka_pick_cube_into_bowl", + "environment": { + "intent": { + "reasoning": "Arena owns this schema", + "background": "maple_table_robolab", + "embodiment": "franka_ik", + "items": [{"query": "cube", "category_tags": ["object"]}], + "initial_state_graph": [], + "tasks": [], + "future_arena_field": {"AutoData": "must not interpret this"}, + } + }, + "planner": { + "backend": "auto", + "motion_backend": "auto", + "collisions": True, + "max_time_s": 60.0, + "batch_size": 128, + "interpolation_dt_s": 0.05, + "profile": False, + "animate": False, + }, + "generation": { + "successful_episodes": 3, + "seed": 7, + "num_envs": 1, + "max_attempts": 9, + }, + "output": { + "dataset": "outputs/data.hdf5", + "keep_failed": False, + "run_log": "outputs/data.jsonl", + }, + } + + +def _parse(data: dict, tmp_path: Path): + return task_request_from_dict(data, source_path=tmp_path / "request.yaml") + + +def _issue_codes(exc: pytest.ExceptionInfo[AutonomousValidationError]) -> set[tuple[str, str]]: + return {(issue.field_path, issue.code) for issue in exc.value.issues} + + +def test_parse_valid_envelope_preserves_opaque_arena_intent(tmp_path: Path): + request = _parse(_request_dict(), tmp_path) + + assert request.schema_version == 1 + assert request.planner.backend is PlannerBackend.AUTO + assert request.planner.motion_backend is MotionBackend.AUTO + assert request.generation.max_attempts == 9 + assert request.output.dataset == "outputs/data.hdf5" + assert request.environment_intent["future_arena_field"] == {"AutoData": "must not interpret this"} + assert len(request.digest) == 64 + + +def test_canonical_request_and_digest_ignore_mapping_insertion_order(tmp_path: Path): + first = _request_dict() + second = dict(reversed(list(copy.deepcopy(first).items()))) + second["environment"]["intent"] = dict(reversed(list(second["environment"]["intent"].items()))) + + request_a = _parse(first, tmp_path) + request_b = _parse(second, tmp_path) + + assert request_a.canonical_json() == request_b.canonical_json() + assert request_a.digest == request_b.digest + + +@pytest.mark.parametrize( + ("container_path", "unknown_key", "expected_path"), + [ + ((), "unexpected", "$.unexpected"), + (("environment",), "robot", "$.environment.robot"), + (("planner",), "timeout", "$.planner.timeout"), + (("generation",), "retry_forever", "$.generation.retry_forever"), + (("output",), "directory", "$.output.directory"), + ], +) +def test_unknown_autodata_keys_are_rejected( + tmp_path: Path, + container_path: tuple[str, ...], + unknown_key: str, + expected_path: str, +): + data = _request_dict() + container = data + for key in container_path: + container = container[key] + container[unknown_key] = "invalid" + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert (expected_path, "unknown_field") in _issue_codes(exc) + + +@pytest.mark.parametrize( + "field_path", + [ + ("planner", "collisions"), + ("planner", "profile"), + ("planner", "animate"), + ("output", "keep_failed"), + ], +) +@pytest.mark.parametrize("invalid", [0, 1, "false", "true"]) +def test_booleans_require_exact_boolean_type(tmp_path: Path, field_path: tuple[str, str], invalid): + data = _request_dict() + data[field_path[0]][field_path[1]] = invalid + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert (f"$.{field_path[0]}.{field_path[1]}", "invalid_type") in _issue_codes(exc) + + +@pytest.mark.parametrize( + ("section", "field", "invalid"), + [ + ("planner", "max_time_s", 0.0), + ("planner", "max_time_s", float("nan")), + ("planner", "batch_size", 0), + ("planner", "batch_size", 1.5), + ("planner", "interpolation_dt_s", float("inf")), + ("generation", "successful_episodes", 0), + ("generation", "seed", -1), + ("generation", "num_envs", 0), + ("generation", "max_attempts", 0), + ], +) +def test_numeric_types_and_ranges_are_strict(tmp_path: Path, section: str, field: str, invalid): + data = _request_dict() + data[section][field] = invalid + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert any(issue.field_path == f"$.{section}.{field}" for issue in exc.value.issues) + + +@pytest.mark.parametrize( + ("field", "value", "choices"), + [ + ("backend", "mimicgen", {"auto", "schedulestream"}), + ("motion_backend", "rrt", {"auto", "curobo_v1", "curobo_v2"}), + ], +) +def test_planner_enums_are_closed(tmp_path: Path, field: str, value: str, choices: set[str]): + data = _request_dict() + data["planner"][field] = value + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + issue = next(issue for issue in exc.value.issues if issue.field_path == f"$.planner.{field}") + assert issue.code == "unsupported_value" + assert all(choice in issue.message for choice in choices) + + +def test_attempt_budget_cannot_be_smaller_than_episode_target(tmp_path: Path): + data = _request_dict() + data["generation"]["successful_episodes"] = 10 + data["generation"]["max_attempts"] = 9 + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert ("$.generation.max_attempts", "attempt_budget_too_small") in _issue_codes(exc) + + +@pytest.mark.parametrize( + ("field", "value", "code"), + [ + ("dataset", "/tmp/data.hdf5", "absolute_path"), + ("dataset", "../data.hdf5", "path_traversal"), + ("dataset", "~/data.hdf5", "home_path"), + ("dataset", "outputs/data.json", "invalid_path_suffix"), + ("run_log", "/tmp/data.jsonl", "absolute_path"), + ("run_log", "../data.jsonl", "path_traversal"), + ("run_log", "outputs/data.log", "invalid_path_suffix"), + ], +) +def test_output_paths_are_relative_bounded_and_typed(tmp_path: Path, field: str, value: str, code: str): + data = _request_dict() + data["output"][field] = value + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert (f"$.output.{field}", code) in _issue_codes(exc) + + +def test_existing_symlink_cannot_escape_request_directory(tmp_path: Path): + request_dir = tmp_path / "request" + outside = tmp_path / "outside" + request_dir.mkdir() + outside.mkdir() + (request_dir / "escape").symlink_to(outside, target_is_directory=True) + data = _request_dict() + data["output"]["dataset"] = "escape/data.hdf5" + + with pytest.raises(AutonomousValidationError) as exc: + task_request_from_dict(data, source_path=request_dir / "request.yaml") + + assert ("$.output.dataset", "path_escape") in _issue_codes(exc) + + +def test_run_log_may_be_omitted_or_null(tmp_path: Path): + omitted = _request_dict() + omitted["output"].pop("run_log") + explicit_null = _request_dict() + explicit_null["output"]["run_log"] = None + + assert _parse(omitted, tmp_path).output.run_log is None + assert _parse(explicit_null, tmp_path).output.run_log is None + + +def test_opaque_intent_rejects_only_non_json_serialization_values(tmp_path: Path): + data = _request_dict() + data["environment"]["intent"]["when"] = Path("not-json") + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert ("$.environment.intent.when", "non_json_intent_value") in _issue_codes(exc) + + +def test_opaque_intent_rejects_non_finite_numbers(tmp_path: Path): + data = _request_dict() + data["environment"]["intent"]["score"] = float("nan") + + with pytest.raises(AutonomousValidationError) as exc: + _parse(data, tmp_path) + + assert ("$.environment.intent.score", "non_finite") in _issue_codes(exc) + + +def test_yaml_duplicate_key_reports_precise_field_path(tmp_path: Path): + example = Path(__file__).parents[3] / "isaac_autodata_examples" / "autonomous" / "franka_pick_cube_into_bowl.yaml" + text = example.read_text(encoding="utf-8") + text = text.replace(" background: maple_table_robolab", " background: first\n background: second") + path = tmp_path / "duplicate.yaml" + path.write_text(text, encoding="utf-8") + + with pytest.raises(AutonomousValidationError) as exc: + load_task_request(path) + + assert ("$.environment.intent.background", "duplicate_key") in _issue_codes(exc) + + +def test_yaml_timestamp_is_rejected_as_non_json_opaque_value(tmp_path: Path): + example = Path(__file__).parents[3] / "isaac_autodata_examples" / "autonomous" / "franka_pick_cube_into_bowl.yaml" + text = example.read_text(encoding="utf-8").replace( + " reasoning: >-", " generated_at: 2026-07-18\n reasoning: >-" + ) + path = tmp_path / "timestamp.yaml" + path.write_text(text, encoding="utf-8") + + with pytest.raises(AutonomousValidationError) as exc: + load_task_request(path) + + assert ("$.environment.intent.generated_at", "non_json_intent_value") in _issue_codes(exc) + + +def test_yaml_12_relation_words_remain_strings(tmp_path: Path): + example = Path(__file__).parents[3] / "isaac_autodata_examples" / "autonomous" / "franka_pick_cube_into_bowl.yaml" + request = load_task_request(example) + + relations = request.environment_intent["initial_state_graph"] + assert [relation["kind"] for relation in relations] == ["is_anchor", "on", "on"] + + +def test_yaml_aliases_are_rejected_before_materialization(tmp_path: Path): + path = tmp_path / "aliases.yaml" + path.write_text( + "schema_version: 1\n" + "name: alias_request\n" + "environment: &environment\n" + " intent: {}\n" + "environment_copy: *environment\n", + encoding="utf-8", + ) + + with pytest.raises(AutonomousValidationError) as exc: + load_task_request(path) + + assert ("$.environment_copy", "yaml_alias_not_allowed") in _issue_codes(exc) + + +def test_yaml_document_size_is_bounded(tmp_path: Path): + path = tmp_path / "large.yaml" + path.write_bytes(b"x" * (MAX_YAML_BYTES + 1)) + + with pytest.raises(AutonomousValidationError) as exc: + load_task_request(path) + + assert ("$", "document_too_large") in _issue_codes(exc) + + +def test_parser_import_does_not_import_arena_isaac_or_torch(): + code = """ +import sys +from isaac_autodata_interfaces.autonomous.task_request import task_request_from_dict +heavy = ('isaaclab_arena', 'isaaclab', 'isaacsim', 'schedulestream', 'curobo', 'torch') +assert not any(name == prefix or name.startswith(prefix + '.') for name in sys.modules for prefix in heavy) +""" + result = subprocess.run( + [sys.executable, "-c", code], + check=False, + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr + + +def test_real_example_parses_without_importing_arena(): + example = Path(__file__).parents[3] / "isaac_autodata_examples" / "autonomous" / "franka_pick_cube_into_bowl.yaml" + + request = load_task_request(example) + + assert request.name == "franka_pick_cube_into_bowl" + assert request.environment_intent["tasks"][0]["kind"] == "PickAndPlaceTask" + assert request.planner.backend is PlannerBackend.SCHEDULESTREAM + assert request.planner.motion_backend is MotionBackend.AUTO diff --git a/isaac_autodata_tests/interfaces/embodiments/test_single_arm_action_scale.py b/isaac_autodata_tests/interfaces/embodiments/test_single_arm_action_scale.py new file mode 100644 index 0000000..fc84194 --- /dev/null +++ b/isaac_autodata_tests/interfaces/embodiments/test_single_arm_action_scale.py @@ -0,0 +1,136 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import math +import torch + +from isaac_autodata_interfaces.embodiments.embodiment_types import PoseObsKeys +from isaac_autodata_interfaces.embodiments.single_arm_embodiment_adapter import DeltaPoseIKSingleArmAdapter + + +class _ActionManager: + active_terms = ("arm_action",) + + def __init__(self, scale: float) -> None: + term_class = type("DifferentialInverseKinematicsAction", (), {}) + self.term = term_class() + self.term._scale = torch.full((1, 6), scale) + + def get_term(self, name: str): + assert name == "arm_action" + return self.term + + +class _Env: + def __init__(self, scale: float) -> None: + self.action_manager = _ActionManager(scale) + self.obs_buf = { + "policy": { + "eef_pos": torch.zeros((1, 3), dtype=torch.float32), + "eef_quat": torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=torch.float32), + } + } + + +def _adapter(scale: float) -> DeltaPoseIKSingleArmAdapter: + adapter = DeltaPoseIKSingleArmAdapter( + name="franka", + eef_name="franka", + pose_obs_keys=PoseObsKeys(pos="eef_pos", quat="eef_quat"), + gripper_action_dim=1, + ) + adapter.bind_env(_Env(scale)) + return adapter + + +def test_action_to_pose_applies_live_ik_action_scale(): + adapter = _adapter(0.5) + action = torch.tensor([[0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) + + target = adapter.action_to_target_eef_pose(action)["franka"] + + assert torch.allclose(target[0, :3, 3], torch.tensor([0.1, 0.0, 0.0])) + + +def test_pose_to_action_divides_by_live_ik_action_scale(): + adapter = _adapter(0.5) + target = torch.eye(4) + target[0, 3] = 0.1 + + action = adapter.target_eef_pose_to_action( + target_eef_pose_dict={"franka": target}, + gripper_action_dict={"franka": torch.tensor([1.0])}, + env_id=0, + ) + + assert torch.allclose(action, torch.tensor([0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]), atol=1e-6) + + +def test_missing_action_manager_preserves_legacy_unit_scale(): + adapter = _adapter(1.0) + adapter.env.action_manager = None + target = torch.eye(4) + target[0, 3] = 0.1 + + action = adapter.target_eef_pose_to_action( + target_eef_pose_dict={"franka": target}, + gripper_action_dict={"franka": torch.tensor([1.0])}, + env_id=0, + ) + + assert torch.allclose(action[:3], torch.tensor([0.1, 0.0, 0.0]), atol=1e-6) + + +def test_gripper_action_extraction_preserves_empty_trailing_dimension(): + adapter = DeltaPoseIKSingleArmAdapter( + name="arm_without_gripper", + eef_name="hand", + pose_obs_keys=PoseObsKeys(pos="eef_pos", quat="eef_quat"), + gripper_action_dim=0, + ) + actions = torch.zeros((2, 3, 6)) + + gripper_actions = adapter.actions_to_gripper_actions(actions)["hand"] + + assert gripper_actions.shape == (2, 3, 0) + + +def test_eef_observation_quaternion_uses_xyzw_scalar_last_order(): + adapter = _adapter(0.5) + half_angle = math.pi / 4.0 + adapter.env.obs_buf["policy"]["eef_quat"][:] = torch.tensor( + [[math.sin(half_angle), 0.0, 0.0, math.cos(half_angle)]] + ) + + pose = adapter.get_eef_poses()["franka"][0] + + expected_rotation = torch.tensor([ + [1.0, 0.0, 0.0], + [0.0, 0.0, -1.0], + [0.0, 1.0, 0.0], + ]) + assert torch.allclose(pose[:3, :3], expected_rotation, atol=1e-6) + + +def test_eef_offset_is_applied_along_the_rotated_observation_local_axis(): + adapter = DeltaPoseIKSingleArmAdapter( + name="franka", + eef_name="franka", + pose_obs_keys=PoseObsKeys(pos="eef_pos", quat="eef_quat"), + gripper_action_dim=1, + eef_offset=(0.0, 0.0, -0.0036), + ) + env = _Env(0.5) + half_angle = math.pi / 4.0 + env.obs_buf["policy"]["eef_pos"][:] = torch.tensor([[1.0, 2.0, 3.0]]) + # FrameTransformer target_quat_w is XYZW; ``_w`` means world-frame coordinates. + env.obs_buf["policy"]["eef_quat"][:] = torch.tensor([[0.0, math.sin(half_angle), 0.0, math.cos(half_angle)]]) + adapter.bind_env(env) + + pose = adapter.get_eef_poses()["franka"][0] + + assert torch.allclose(pose[:3, 3], torch.tensor([1.0036, 2.0, 3.0]), atol=1e-6) diff --git a/isaac_autodata_tests/interfaces/motion_planners/test_backend_selection.py b/isaac_autodata_tests/interfaces/motion_planners/test_backend_selection.py new file mode 100644 index 0000000..07b8d68 --- /dev/null +++ b/isaac_autodata_tests/interfaces/motion_planners/test_backend_selection.py @@ -0,0 +1,107 @@ +# Copyright (c) 2026, The Isaac AutoData Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: Apache-2.0 + +from __future__ import annotations + +import pytest + +from isaac_autodata_interfaces.motion_planners.curobo.backend_selection import ( + CUROBO_V1_MARKERS, + CUROBO_V2_MARKERS, + SCHEDULESTREAM_V1_MARKERS, + SCHEDULESTREAM_V2_MARKERS, + BackendCompatibilityError, + CuroboApiGeneration, + DistributionIdentity, + _sanitize_source_url, + detect_curobo_runtime, + select_schedulestream_backend, +) + + +def _detect(available: set[str]): + return detect_curobo_runtime( + module_available=lambda name: name in available, + distribution_reader=lambda names: DistributionIdentity(names[0], "test"), + ) + + +def test_detects_curobo_v1_and_custream(): + capabilities = _detect({"curobo", "schedulestream", *CUROBO_V1_MARKERS, *SCHEDULESTREAM_V1_MARKERS}) + + assert capabilities.api_generation is CuroboApiGeneration.V1 + selection = select_schedulestream_backend("auto", capabilities) + assert selection.motion_backend == "curobo_v1" + assert selection.schedulestream_application == "custream" + + +def test_detects_curobo_v2_and_custream2(): + capabilities = _detect({"curobo", "schedulestream", *CUROBO_V2_MARKERS, *SCHEDULESTREAM_V2_MARKERS}) + + assert capabilities.api_generation is CuroboApiGeneration.V2 + selection = select_schedulestream_backend("curobo_v2", capabilities) + assert selection.schedulestream_application == "custream2" + + +def test_v2_markers_win_when_compatibility_v1_modules_are_also_present(): + capabilities = _detect({ + "curobo", + "schedulestream", + *CUROBO_V1_MARKERS, + *CUROBO_V2_MARKERS, + *SCHEDULESTREAM_V1_MARKERS, + *SCHEDULESTREAM_V2_MARKERS, + }) + + assert capabilities.api_generation is CuroboApiGeneration.V2 + assert capabilities.schedulestream_application == "custream2" + + +def test_rejects_requested_api_that_does_not_match_runtime(): + capabilities = _detect({"curobo", "schedulestream", *CUROBO_V1_MARKERS, *SCHEDULESTREAM_V1_MARKERS}) + + with pytest.raises(BackendCompatibilityError, match="provides curobo_v1"): + select_schedulestream_backend("curobo_v2", capabilities) + + +def test_reports_missing_matching_schedulestream_application(): + capabilities = _detect({"curobo", *CUROBO_V1_MARKERS}) + + with pytest.raises(BackendCompatibilityError, match="custream"): + select_schedulestream_backend("auto", capabilities) + + +def test_reports_unavailable_curobo_without_importing_it(): + capabilities = _detect(set()) + + assert capabilities.api_generation is CuroboApiGeneration.UNAVAILABLE + with pytest.raises(BackendCompatibilityError, match="No supported cuRobo API"): + select_schedulestream_backend("auto", capabilities) + + +def test_current_host_probe_is_serializable(): + # This is deliberately not an acceptance assertion about which backend the developer has. + # It verifies that real filesystem/metadata probing is safe and produces runtime identity data. + capabilities = detect_curobo_runtime() + payload = capabilities.to_dict() + + assert payload["api_generation"] in {item.value for item in CuroboApiGeneration} + assert "missing_markers" in payload + + +@pytest.mark.parametrize( + ("raw_url", "expected"), + [ + ( + "https://robot:secret@example.com:8443/org/repo.git?token=private#fragment", + "https://example.com:8443/org/repo.git", + ), + ("file:///home/engineer/private/schedulestream", "file:///"), + ("/home/engineer/private/schedulestream", None), + ("https://example.com/repo.git\nAuthorization: secret", None), + ], +) +def test_direct_install_url_is_sanitized_before_runtime_identity(raw_url: str, expected: str | None) -> None: + assert _sanitize_source_url(raw_url) == expected diff --git a/submodules/IsaacLab-Arena b/submodules/IsaacLab-Arena index 754c87a..8a74e79 160000 --- a/submodules/IsaacLab-Arena +++ b/submodules/IsaacLab-Arena @@ -1 +1 @@ -Subproject commit 754c87a691a6e26d481fdc089c84c2f70262f0c5 +Subproject commit 8a74e794b621b0f8d3627d096a1bae9ce11e7b56