Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions agent_context/topics/atomic-actions/affordance-sampling.md
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,12 @@ never retain it. `AffordancePoseCandidates` retains poses, costs, and an
explicit validity mask. `AffordanceSample` returns row-local success, poses,
and owned metadata.

A caller can supply keyed `preferred_local_poses` from a previously verified
episode. A non-nominal pose selection may match one of the current valid,
geometrically distinct alternatives in the same reference frame. The ordinary
rank applies when no equivalent alternative exists. The sample records the
current candidate ID and whether it matched. The caller owns selection history.

Actions own IK, reachability, complete trajectory feasibility, and diagnostics.
They intersect `AffordanceSample.success` with their feasibility result and put
the sampling record under `diagnostics.metadata["affordance_sample"]`.
Expand Down
24 changes: 24 additions & 0 deletions agent_context/topics/atomic-actions/execution.md
Original file line number Diff line number Diff line change
Expand Up @@ -278,3 +278,27 @@ unchanged. HandOver can explicitly retract
sampled grasp TCPs through `grasp_retract_distance` before deriving transport
and handover targets. The offset is a planning declaration; acquisition,
retention and final acceptance still require the original physical evidence.

## Tolerance-bounded invocation handoff

`TrackingPolicy.boundary_stop` adds a joint-motion stop gate without replacing
timed/feedback terminal acceptance or effect verification. It retains the exact
terminal command, including hand targets, until two distinct observations satisfy
the configured target error and control-grid measured-position speed tolerances.
Native qvel remains a finite-data check and is logged separately; its post-substep
value is not substituted for the control-grid derivative. Invalid rows and physical
timeouts fail closed without keeping failed rows in another terminal gate's mask.
The state resets on plan installation/recovery; snapshots preserve independent
ownership. This is not strict reference continuity, C1/C2 blending or a zero
tracking-error guarantee. `boundary_diagnostic` events identify tolerances, elapsed
physical time and per-row error/speed/readiness.

`boundary_reference_bridge` optionally separates controller-reference continuity
from measured state. Native free entry phases can receive a bounded rest-to-rest
prefix from captured drive targets to their original first sample. The original
phase name, held/contact guards and suffix remain; lengths/monitor indices shift
together. Prefix and join stencils are protected from expansion and retiming;
rebuilding cannot prepend twice. Projected observations advance owned references
without overwriting measured evidence. External nominal/unsupported phases retain
existing behavior with explicit unavailable diagnostics. Physical qualification
remains required after collision certificates are invalidated.
18 changes: 18 additions & 0 deletions agent_context/topics/expansion-engine/expansion-engine.md
Original file line number Diff line number Diff line change
Expand Up @@ -239,3 +239,21 @@ The report distinguishes cumulative additions from independent cohorts; observed
bins are not a feasible-volume certificate. The reader uses existing artifact
validation dependencies but creates no World. See
[distribution usage](../../../embodichain/lab/expansion/distribution.md).

Native configured jobs snapshot execution policies through the canonical selector
resolver and default `tracking.boundary_stop` on. Supported native free entry
phases also inherit `boundary_reference_bridge` from that setting; explicit
flags retain their values. Atomic planning owns the bounded prefix and protected
join, not a postprocessing filter. Preserve explicit opt-out and
component overrides. They do not mutate authored policy files or replace physical
validators. The Atomic terminal gate owns additional real command frames; these
consume ordinary rollout budgets and T+1 evidence. See
[invocation handoff](../atomic-actions/execution.md#tolerance-bounded-invocation-handoff)
and [boundary diagnostics](../../../examples/expansion/engine/skill-boundaries.md).

Native joint-residual and via-point generation retain their original proposal budgets and
full-amplitude preference. On exhaustion, the adapter may deterministically
shrink one closest motion rejection under unchanged minimum-delta, joint, motion,
FK and physical checks. It preserves the time grid and protected samples; metadata
records the raw proposal count, refinement scale and both validation results.
See [boundary and residual qualification](../../../examples/expansion/engine/skill-boundaries.md).
24 changes: 22 additions & 2 deletions agent_context/topics/expansion-engine/workloads.md
Original file line number Diff line number Diff line change
Expand Up @@ -143,8 +143,11 @@ Default CPU command gathering shares native numeric reads through a bounded
World-owner scope in `sim/objects/backends/_numeric_observations.py`. It ends
before completion callbacks and physical stepping, and starts after reset and
activation. Checked native writes, physics steps and binding topology changes
invalidate samples. Derived dynamic rigid poses share the same boundary and invalidation, with
separate CPU autocast representations. Consumers receive independent output
invalidate samples. Requests for matrices of a named articulation link can read
just the selected CPU arenas; the result uses the same arena frame and float32
rotation conversion as the full link batch. Derived dynamic rigid poses share
the same boundary and invalidation, with separate CPU autocast representations.
Consumers receive independent output
buffers; semantic
state, episode clocks and validation remain row-owned. CUDA and other backends
retain native reads. Raw SDK mutation or concurrent World writes inside the
Expand Down Expand Up @@ -186,6 +189,23 @@ switch devices.

## Optional task-pose proposal feedback

For native Task Program jobs, `trajectory.affordance.branch_selection` defaults
to `round_robin`. With `outcome_guided`, the workload assigns each non-nominal
grasp rank once before using physical outcomes to rank future choices. Once a failure
has been observed, ranks are scored by (successes + 1) / (trials + 2).
If every rank has a physical failure and none has finished successfully, the
one with the most verified Task Program segments takes priority.
`scheduling.exploration_fraction` (default 0.25) reserves a fraction of the
remaining selections for the least-used ranks. Each motion recipe records its
chosen rank before execution. After a successful branch, exploitation can also
record its source ordinal and accepted object-relative grasp matrices. Each
selected call looks for an equivalent non-nominal pose in its freshly generated
valid candidates and replans against the new scene. Exploration uses fresh
ranked choices. Selected calls in the recipe reuse its declared branch.
Only physical outcomes with a recorded plan transform count once per attempt.
Preparation failures, queue limits, and cancelled rows supply no evidence.
Feedback is local to one workload run.

Task-facing expansion configs select `scene_sampling.strategy` (`bounded_pose`
or `workspace_joint`) independently of `scheduling.policy` (`fifo` or
`task_pose_coverage`). Absent settings preserve the legacy bounded sampler.
Expand Down
11 changes: 10 additions & 1 deletion embodichain/lab/expansion/_task_program_config.py
Original file line number Diff line number Diff line change
Expand Up @@ -203,11 +203,20 @@ def load_task_program_expansion(path: str | Path) -> dict[str, Any]:
if (
not isinstance(affordance, dict)
or required_affordance - set(affordance)
or set(affordance) - required_affordance - {"grasp_generators"}
or set(affordance)
- required_affordance
- {"grasp_generators", "branch_selection"}
):
raise ValueError("Invalid trajectory.affordance settings")
if type(affordance["branches"]) is not int or affordance["branches"] < 2:
raise ValueError("trajectory.affordance.branches must be at least two")
if affordance.get("branch_selection", "round_robin") not in (
"round_robin",
"outcome_guided",
):
raise ValueError(
"affordance.branch_selection must be round_robin or outcome_guided"
)
calls = affordance["call_indices"]
if (
not isinstance(calls, list)
Expand Down
19 changes: 19 additions & 0 deletions embodichain/lab/expansion/gym_workloads.py
Original file line number Diff line number Diff line change
Expand Up @@ -489,6 +489,25 @@ def complete(self, result: WorkloadRowResult) -> Any:
receipt = None
try:
receipt = group["engine"].complete_row(state["native"], episode)
if (
ctx.extensions.plan_outcome is not None
and result.world_control_start is not None
and outcome.expansion_records
and outcome.expansion_records[0]
and outcome.reasons[0]
in (
"completed",
"program_rejected",
"segment_rejected",
"measured_validation_failed",
"environment_terminated",
)
):
ctx.extensions.plan_outcome(
result.assignment.ordinal,
episode is not None and episode.validation.accepted,
outcome,
)
return receipt
finally:
if ctx.feedback is not None:
Expand Down
11 changes: 10 additions & 1 deletion embodichain/lab/expansion/host_extensions.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,13 @@
from dataclasses import dataclass, field
from collections.abc import Callable, Mapping
from pathlib import Path
from typing import Any, Protocol
from typing import Any, Protocol, TYPE_CHECKING
from .contracts import CandidateIdentity, SceneCase, ValidationResult
from .recipes import ExpansionRecipe

if TYPE_CHECKING:
from .program import ProgramBatchResult

__all__ = ["ExpansionValidator", "ExpansionRowContext", "GymExpansionHostExtensions"]


Expand Down Expand Up @@ -87,6 +90,8 @@ class GymExpansionHostExtensions:
scene source and artifact directory; exposes coverage, budget, validate,
finish and checkpoint without advancing physics or writing episodes.
require_plan_transform: Reject completed programs without transform records.
plan_outcome: Receives the logical ordinal, physical acceptance and
single-row program result after a rollout with applied plan evidence.
"""

collection_renderer: str | None = None
Expand All @@ -98,6 +103,9 @@ class GymExpansionHostExtensions:
default=None, kw_only=True
)
require_plan_transform: bool = False
plan_outcome: Callable[[int, bool, ProgramBatchResult], None] | None = field(
default=None, kw_only=True
)

def __post_init__(self) -> None:
if self.collection_renderer is not None and (
Expand All @@ -111,6 +119,7 @@ def __post_init__(self) -> None:
"validator_factory",
"recipe_transform",
"plan_transform_factory",
"plan_outcome",
):
value = getattr(self, name)
if value is not None and not callable(value):
Expand Down
142 changes: 142 additions & 0 deletions embodichain/lab/expansion/task_program_jobs.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
"""Declarative scene/motion jobs composed through the native workload engine."""

from __future__ import annotations
from collections import Counter
from collections.abc import Mapping, Sequence
from copy import deepcopy
from dataclasses import replace
Expand All @@ -34,6 +35,7 @@

if TYPE_CHECKING:
from embodichain.lab.sim.scene.feasibility import JointSceneCandidate
from .program import ProgramBatchResult

__all__ = [
"load_task_program_expansion",
Expand All @@ -58,11 +60,114 @@ def _load_deployment(path: Path) -> None:
from ._pose_sampling import _BoundedPoseSource, _AdaptivePoseSource


def _accepted_local_poses(
records: Sequence[Mapping[str, Any]],
) -> dict[str, list[float]]:
"""Express each recorded grasp in its own measured reference frame."""
import numpy as np

poses = {}
for record in records:
for sample in record.get("samples", {}).values():
if (
sample.get("pose_frame") == "world"
and sample.get("success") == [True]
and sample.get("reference_poses") is not None
and sample.get("selected_poses") is not None
and sample.get("key")
):
local = np.linalg.solve(
sample["reference_poses"][0], sample["selected_poses"][0]
)
poses[sample["key"]] = local.reshape(16).tolist()
return poses


class _AffordanceBranchSchedule:
"""Select non-nominal branches using measured outcomes from one workload."""

def __init__(self, count: int, exploration_fraction: float) -> None:
self.count = count - 1 # Branch zero is the nominal grasp.
self.exploration_fraction = exploration_fraction
self.assignments: dict[int, int] = {}
self.allocations: Counter[int] = Counter()
self.successes: Counter[int] = Counter()
self.failures: Counter[int] = Counter()
self.verified_segments: Counter[int] = Counter()
self.finished: set[int] = set()
self.exemplars: dict[int, dict] = {}
self.preferences: dict[int, dict] = {}

def select(self, ordinal: int) -> int:
"""Freeze one choice per recipe, including concurrently admitted rows."""
if ordinal in self.assignments:
return self.assignments[ordinal]
if len(self.allocations) < self.count:
branch = len(self.allocations) + 1
else:
n = len(self.assignments) - self.count
explore = int((n + 1) * self.exploration_fraction) > int(
n * self.exploration_fraction
)

compare_progress = not self.successes and len(self.failures) == self.count

def priority(branch: int) -> tuple[int, float, int, int]:
wins = self.successes[branch]
# Beta(1, 1) prior. Only observed physical outcomes change it.
rate = (1 + wins) / (2 + wins + self.failures[branch])
return (
(
self.verified_segments[branch]
if compare_progress and not explore
else 0
),
rate if self.failures and not explore else 0.0,
-self.allocations[branch],
-branch,
)

branch = max(self.allocations, key=priority)
if not explore and self.failures and branch in self.exemplars:
self.preferences[ordinal] = self.exemplars[branch]
self.assignments[ordinal] = branch
self.allocations[branch] += 1
return branch

def observe(self, ordinal: int, accepted: bool, result: ProgramBatchResult) -> None:
"""Credit an applied branch once, independently of dataset admission."""
if ordinal not in self.finished:
branch = self.assignments[ordinal]
counts = self.successes if accepted else self.failures
counts[branch] += 1
verified_segments = sum(
segment.get("success") is True for segment in result.segments[0]
)
self.verified_segments[branch] = max(
self.verified_segments[branch], verified_segments
)
if accepted:
poses = _accepted_local_poses(result.expansion_records[0])
if poses:
self.exemplars[branch] = {"ordinal": ordinal, "local_poses": poses}
self.finished.add(ordinal)


def _task_expansion_extensions(*, options, operator, seed, report):
"""Declare task proposals; upstream owns collection and persistence."""
from .host_extensions import GymExpansionHostExtensions

pose_source = None
affordance = options["trajectory"].get("affordance", {})
branches = None
if (
operator == "affordance"
and affordance.get("branch_selection") == "outcome_guided"
):
branches = _AffordanceBranchSchedule(
affordance["branches"],
options.get("scheduling", {}).get("exploration_fraction", 0.25),
)

def source(env):
nonlocal pose_source
Expand Down Expand Up @@ -92,6 +197,14 @@ def recipe_transform(recipe):
payload = json.loads(operation.payload_json)
if operation.name == "motion":
payload.update(operator=operator, options=options["trajectory"])
if branches is not None:
payload["affordance_branch"] = branches.select(
recipe.context.ordinal
)
if (
prior := branches.preferences.get(recipe.context.ordinal)
) is not None:
payload["accepted_grasps"] = prior
elif operation.name == "joint_scene":
candidate = pose_source.candidate(recipe.context.ordinal)
payload["pose_changes"] = {
Expand All @@ -106,6 +219,10 @@ def plans(env, row):
TaskProgramSameGridExpansionFactory,
)

payload = {}
if branches is not None:
motion = next(op for op in row.recipe.operations if op.name == "motion")
payload = json.loads(motion.payload_json)
return TaskProgramSameGridExpansionFactory.from_native_plan(
operator,
options["trajectory"],
Expand All @@ -114,6 +231,13 @@ def plans(env, row):
revision=report["recipe_revision"],
output=row.directory / "plans",
episode_id=row.ordinal,
affordance_branch=payload.get("affordance_branch"),
preferred_local_poses=tuple(
(key, tuple(pose))
for key, pose in payload.get("accepted_grasps", {})
.get("local_poses", {})
.items()
),
)

return GymExpansionHostExtensions(
Expand All @@ -122,6 +246,7 @@ def plans(env, row):
recipe_transform=recipe_transform,
plan_transform_factory=plans,
require_plan_transform=operator != "object_pose",
plan_outcome=None if branches is None else branches.observe,
)


Expand Down Expand Up @@ -159,6 +284,23 @@ def paths(value):
{owner: snapshot[owner]}, base_dir=deployment.parent
).config
(output / "environment.yaml").write_text(yaml.safe_dump(physical, sort_keys=False))
policy_path = snapshot.get("task_program", {}).get("execution_policy")
if policy_path is not None:
from embodichain.lab.task_program.integrations._configured_composition import (
_resolve_execution_policy,
)

_, policy = _resolve_execution_policy(policy_path, base_dir=deployment.parent)
policy = deepcopy(policy)
policy.setdefault("tracking", {"kind": "timed"}).setdefault(
"boundary_stop", True
)
policy["tracking"].setdefault(
"boundary_reference_bridge", policy["tracking"]["boundary_stop"]
)
boundary_policy = output / "execution_policy.yaml"
boundary_policy.write_text(yaml.safe_dump(policy, sort_keys=False))
snapshot["task_program"]["execution_policy"] = str(boundary_policy.resolve())
snapshot["id"] += "-Expansion-v1"
path = output / "deployment.json"
atomic_json(path, snapshot)
Expand Down
Loading
Loading