diff --git a/agent_context/topics/atomic-actions/affordance-sampling.md b/agent_context/topics/atomic-actions/affordance-sampling.md index 681120273..26899545d 100644 --- a/agent_context/topics/atomic-actions/affordance-sampling.md +++ b/agent_context/topics/atomic-actions/affordance-sampling.md @@ -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"]`. diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 637262466..0abef37f6 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -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. diff --git a/agent_context/topics/expansion-engine/expansion-engine.md b/agent_context/topics/expansion-engine/expansion-engine.md index 14eca61ae..a05238cb7 100644 --- a/agent_context/topics/expansion-engine/expansion-engine.md +++ b/agent_context/topics/expansion-engine/expansion-engine.md @@ -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). diff --git a/agent_context/topics/expansion-engine/workloads.md b/agent_context/topics/expansion-engine/workloads.md index 7e9e1b758..be0ad7afe 100644 --- a/agent_context/topics/expansion-engine/workloads.md +++ b/agent_context/topics/expansion-engine/workloads.md @@ -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 @@ -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. diff --git a/embodichain/lab/expansion/_task_program_config.py b/embodichain/lab/expansion/_task_program_config.py index 40ff525a1..bad7d7619 100644 --- a/embodichain/lab/expansion/_task_program_config.py +++ b/embodichain/lab/expansion/_task_program_config.py @@ -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) diff --git a/embodichain/lab/expansion/gym_workloads.py b/embodichain/lab/expansion/gym_workloads.py index 44d2cd8d7..e64cae3ee 100644 --- a/embodichain/lab/expansion/gym_workloads.py +++ b/embodichain/lab/expansion/gym_workloads.py @@ -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: diff --git a/embodichain/lab/expansion/host_extensions.py b/embodichain/lab/expansion/host_extensions.py index 911e1e67b..8b0121d8e 100644 --- a/embodichain/lab/expansion/host_extensions.py +++ b/embodichain/lab/expansion/host_extensions.py @@ -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"] @@ -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 @@ -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 ( @@ -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): diff --git a/embodichain/lab/expansion/task_program_jobs.py b/embodichain/lab/expansion/task_program_jobs.py index 728965261..0348aee01 100644 --- a/embodichain/lab/expansion/task_program_jobs.py +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -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 @@ -34,6 +35,7 @@ if TYPE_CHECKING: from embodichain.lab.sim.scene.feasibility import JointSceneCandidate + from .program import ProgramBatchResult __all__ = [ "load_task_program_expansion", @@ -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 @@ -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"] = { @@ -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"], @@ -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( @@ -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, ) @@ -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) diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 9156113f0..f5e09673c 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -1616,6 +1616,8 @@ def _segment_progress_total_steps(self, segment: Any) -> int | None: or recovery.max_skill_retries != 0 or preset.workflow_recovery_policy.max_recovery_attempts != 0 or tracking.in_flight is not None + or tracking.boundary_stop + or tracking.boundary_reference_bridge or getattr(tracking.terminal, "settle_duration", None) != 0.0 ): return None diff --git a/embodichain/lab/sim/atomic_skills/_plan_expansion.py b/embodichain/lab/sim/atomic_skills/_plan_expansion.py index 53181bbb0..9d0ca5273 100644 --- a/embodichain/lab/sim/atomic_skills/_plan_expansion.py +++ b/embodichain/lab/sim/atomic_skills/_plan_expansion.py @@ -246,6 +246,29 @@ def _expand_plan( ) ), ) + reference_bridge = plan.diagnostics.metadata.get("reference_bridge") + if reference_bridge: + # The takeover reference and its join stencil are not augmentation DOFs. + protected = int(reference_bridge["prefix_samples"]) + 2 + phases = tuple( + ( + replace(phase, start_index=max(phase.start_index, protected)) + if phase.stop_index > max(phase.start_index, protected) + 2 + else replace(phase, allowed_operators=()) + ) + for phase in template.phases + ) + template = replace( + template, + phases=phases, + allowed_operators=tuple( + dict.fromkeys( + operator + for phase in phases + for operator in phase.allowed_operators + ) + ), + ) if plan.diagnostics.metadata.get("demo_skill") is not None: _, _, _, source_to_canonical = _canonical_samples( trajectory.positions[row], trajectory.dt[row] diff --git a/embodichain/lab/sim/atomic_skills/_reference_bridge.py b/embodichain/lab/sim/atomic_skills/_reference_bridge.py new file mode 100644 index 000000000..bdade5fc8 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/_reference_bridge.py @@ -0,0 +1,48 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Plan a rest-to-rest command-reference handoff without changing observations.""" + +from __future__ import annotations +import torch +from embodichain.compute.trajectory import interpolate_minimum_jerk + +__all__: list[str] = [] + + +def _reference_prefix( + reference: torch.Tensor, + initial: torch.Tensor, + ids: tuple[int, ...], + dt: float, + velocity: torch.Tensor, + acceleration: float, +) -> torch.Tensor: + """Return an owned prefix excluding the original initial sample.""" + start = initial.clone() + start[:, list(ids)] = reference[:, list(ids)] + if torch.allclose(start, initial, atol=1e-7, rtol=0): + return initial[:, None, :][:, :0].clone() + controlled = interpolate_minimum_jerk( + torch.stack((start[:, list(ids)], initial[:, list(ids)]), dim=1), + 3, + dt, + velocity[list(ids)], + acceleration, + ) + path = initial[:, None, :].expand(-1, controlled.shape[1], -1).clone() + path[:, :, list(ids)] = controlled + return path[:, :-1].clone() diff --git a/embodichain/lab/sim/atomic_skills/affordance_sampling.py b/embodichain/lab/sim/atomic_skills/affordance_sampling.py index 3691c8889..3ef4e205c 100644 --- a/embodichain/lab/sim/atomic_skills/affordance_sampling.py +++ b/embodichain/lab/sim/atomic_skills/affordance_sampling.py @@ -33,6 +33,10 @@ ] +_POSITION_EQUIVALENCE_M = 1.0e-4 +_ROTATION_EQUIVALENCE_FROBENIUS = 1.0e-3 + + @dataclass(frozen=True, slots=True) class AffordanceSamplingContext: """Immutable identity for reproducible parallel Affordance sampling. @@ -44,6 +48,9 @@ class AffordanceSamplingContext: attempt_id: Explicit resampling attempt identity. branch_override: Optional logical branch selected independently of the physical environment row. + preferred_local_poses: Previously accepted reference-relative poses by + sampling key, stored as immutable row-major matrices. A match must + be a distinct non-nominal candidate in the current valid set. """ count: int = 1 @@ -52,6 +59,7 @@ class AffordanceSamplingContext: attempt_id: int = 0 branch_override: int | None = None branch_overrides: tuple[int, ...] | None = None + preferred_local_poses: tuple[tuple[str, tuple[float, ...]], ...] = () def __post_init__(self) -> None: if type(self.count) is not int or self.count < 1: @@ -77,6 +85,22 @@ def __post_init__(self) -> None: raise ValueError( "branch_override and branch_overrides are mutually exclusive" ) + if self.preferred_local_poses: + poses = {} + for key, values in self.preferred_local_poses: + pose = tuple(values) + if ( + not isinstance(key, str) + or not key + or key in poses + or len(pose) != 16 + or not all(map(math.isfinite, pose)) + ): + raise ValueError( + "preferred_local_poses requires unique keys and finite 4x4 matrices" + ) + poses[key] = pose + object.__setattr__(self, "preferred_local_poses", tuple(poses.items())) @property def enabled(self) -> bool: @@ -168,7 +192,7 @@ def sample_range( values[row] = lower + (upper - lower) * float(groups[group][branch - 1]) return values - def metadata(self) -> dict[str, int | None]: + def metadata(self) -> dict[str, object]: """Return JSON-compatible sampling provenance.""" return { "count": self.count, @@ -177,6 +201,11 @@ def metadata(self) -> dict[str, int | None]: "attempt_id": self.attempt_id, "branch_override": self.branch_override, "branch_overrides": self.branch_overrides, + **( + {"preferred_local_poses": self.preferred_local_poses} + if self.preferred_local_poses + else {} + ), } @@ -343,6 +372,12 @@ def _sample_pose_candidates( success = eligible.any(dim=1) unique_counts: list[int] = [] reused: list[bool] = [] + preferred = ( + dict(sampling.preferred_local_poses).get(key) + if sampling is not None and reference_poses is not None + else None + ) + matched = [False] * len(env_ids) if preferred is not None else [] for row, env_id in enumerate(env_ids.cpu().tolist()): indices = torch.argsort(masked_costs[row], stable=True) @@ -391,7 +426,8 @@ def _sample_pose_candidates( rotation - rotation[last], dim=1 ) available &= ~( - (position_distance <= 1.0e-4) & (rotation_distance <= 1.0e-3) + (position_distance <= _POSITION_EQUIVALENCE_M) + & (rotation_distance <= _ROTATION_EQUIVALENCE_FROBENIUS) ) nearest = torch.minimum( nearest, position_distance / scale + rotation_distance @@ -401,7 +437,25 @@ def _sample_pose_candidates( priority = nearest * (0.75 + 0.25 * jitter) order.append(int(priority.masked_fill(~available, -torch.inf).argmax())) branch = sampling.branch_for(env_id, row_index=row) - selected[row] = indices[order[branch % len(order)]] + choice = order[branch % len(order)] + if preferred is not None and 0 < branch < len(order): + prior = position.new_tensor(preferred).reshape(4, 4) + alternatives = order[1:] + delta_p = torch.linalg.vector_norm( + position[alternatives] - prior[:3, 3], dim=1 + ) + delta_r = torch.linalg.vector_norm( + rotation[alternatives] - prior[:3, :3].reshape(9), dim=1 + ) + # The same equivalence tolerances distinguish candidates above. + equivalent = (delta_p <= _POSITION_EQUIVALENCE_M) & ( + delta_r <= _ROTATION_EQUIVALENCE_FROBENIUS + ) + if equivalent.any(): + error = (delta_p / scale + delta_r).masked_fill(~equivalent, torch.inf) + choice = alternatives[int(error.argmin())] + matched[row] = True + selected[row] = indices[choice] unique_counts.append(len(order)) reused.append(branch >= len(order)) @@ -422,6 +476,11 @@ def _sample_pose_candidates( "valid_candidate_counts": eligible.sum(1).cpu().tolist(), "unique_candidate_counts": unique_counts, "reused": reused, + **( + {"matched_preferred_local_pose": matched} + if preferred is not None + else {} + ), }, ) diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 3f1c0adb2..096bb0396 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -1047,6 +1047,215 @@ def build_plan( if not preserve_failed_positions: timed = timed.hold_rows(success_mask, context.robot.qpos) + from ._expansion_phases import FREE_PHASES + + already_bridged = diagnostics is not None and bool( + diagnostics.metadata.get("reference_bridge") + ) + bridge_supported = ( + next(iter(segment_lengths or {}), None) + in FREE_PHASES.get(request.skill_id, ()) + and request.motion_policy.acceleration_limit is not None + # Position-only transports may expose zero feedforward velocity + # during motion. Do not apply this rest handoff to action recovery. + and request.recovery_policy.max_replans == 0 + and request.recovery_policy.max_skill_retries == 0 + and request.revision == 0 + and bool(success_mask.any()) + and not ( + diagnostics is not None and diagnostics.backend == "external_nominal" + ) + ) + if ( + request.tracking_policy.boundary_reference_bridge + and not already_bridged + and context.robot.command_qvel is not None + ): + motion_ids = tuple( + dict.fromkeys( + i + for e in request.binding.endpoints + if e.endpoint_id == "motion" + for i in e.joint_ids + ) + ) + if motion_ids and bool( + ( + context.robot.command_qvel[success_mask][:, list(motion_ids)].abs() + > 1e-6 + ).any() + ): + bridge_supported = False + if ( + request.tracking_policy.boundary_reference_bridge + and not bridge_supported + and not already_bridged + ): + previous = diagnostics or PlannerDiagnostics(backend="reference_takeover") + diagnostics = replace( + previous, + metadata={ + **previous.metadata, + "reference_bridge_unavailable": "unsupported_phase_source_or_boundary_conditions", + }, + ) + if ( + request.tracking_policy.boundary_reference_bridge + and bridge_supported + and not ( + diagnostics is not None and diagnostics.metadata.get("reference_bridge") + ) + ): + from ._reference_bridge import _reference_prefix + from ._expansion_phases import FREE_PHASES + + lengths = dict(segment_lengths or {}) + first = next(iter(lengths), None) + if first not in FREE_PHASES.get(request.skill_id, ()): + raise ValueError( + "Reference takeover requires an explicitly free first phase" + ) + reference = context.robot.command_qpos + reference_velocity = context.robot.command_qvel + if reference is None or reference_velocity is None: + raise ValueError( + "Reference takeover requires measured controller target snapshots" + ) + ids = tuple( + dict.fromkeys( + i + for e in request.binding.endpoints + if e.endpoint_id == "motion" + for i in e.joint_ids + ) + ) + if not ids or bool( + (reference_velocity[success_mask][:, list(ids)].abs() > 1e-6).any() + ): + raise ValueError( + "Reference takeover requires stationary motion targets" + ) + dt = context.require_control_dt() + limits = ( + self.robot.get_qvel_limits(env_ids=context.env_ids)[success_mask] + .amin(dim=0) + .to(timed.positions) + ) + if request.motion_policy.velocity_limit is not None: + limits = limits.clamp(max=request.motion_policy.velocity_limit) + acceleration = request.motion_policy.acceleration_limit + if acceleration is None: + raise ValueError( + "Reference takeover requires an explicit acceleration bound" + ) + reference = torch.where( + success_mask[:, None], reference, timed.positions[:, 0] + ) + prefix = _reference_prefix( + reference, timed.positions[:, 0], ids, dt, limits, acceleration + ) + count = prefix.shape[1] + if count: + positions = torch.cat((prefix, timed.positions), dim=1) + times = torch.cat( + ( + timed.dt.new_full((timed.batch_size, count), dt), + timed.dt.clone(), + ), + dim=1, + ) + times[:, 0] = 0 + times[:, count] = dt + window = positions[:, : count + min(3, timed.waypoint_count), list(ids)] + window_dt = times[:, : window.shape[1]] + speed = torch.diff(window, dim=1) / window_dt[:, 1:, None] + accel = torch.diff(speed, dim=1) / window_dt[:, 2:, None] + joint_limits = self.robot.get_qpos_limits( + joint_ids=list(ids), env_ids=context.env_ids + ).to(window) + if ( + joint_limits.shape != (timed.batch_size, len(ids), 2) + or bool(torch.isnan(joint_limits).any()) + or bool((joint_limits[..., 0] > joint_limits[..., 1]).any()) + ): + raise ValueError( + "Reference takeover requires valid ordered joint limits" + ) + if bool( + ( + (window < joint_limits[:, None, :, 0] - 1e-6) + | (window > joint_limits[:, None, :, 1] + 1e-6) + )[success_mask].any() + ): + raise ValueError("Reference takeover exceeds joint limits") + if bool( + ( + speed[success_mask].abs() > limits[list(ids)][None, None] + 1e-5 + ).any() + ) or bool((accel[success_mask].abs() > acceleration + 1e-5).any()): + raise ValueError( + "Reference takeover seam exceeds velocity/acceleration bounds" + ) + original_velocity = timed.velocities + if original_velocity is None: + original_velocity = differentiate_positions( + timed.positions, timed.dt + ) + original_velocity = original_velocity.clone() + original_velocity[:, 0] = 0 + original_velocity[:, -1] = 0 + bridge_points = torch.cat((prefix, timed.positions[:, :1]), dim=1) + bridge_dt = times[:, : count + 1] + bridge_velocity = differentiate_positions(bridge_points, bridge_dt) + velocity = torch.cat( + (bridge_velocity[:, :-1], original_velocity), dim=1 + ) + timed = TimedTrajectory.from_positions( + positions, env_ids=timed.env_ids, dt=times, velocities=velocity + ) + segment_lengths = dict(lengths) + segment_lengths[first] += count + if scene_dependency_monitor_until is not None: + scene_dependency_monitor_until = { + key: (value + count if value else 0) + for key, value in scene_dependency_monitor_until.items() + } + previous = diagnostics or PlannerDiagnostics( + backend="reference_takeover" + ) + diagnostics = replace( + previous, + metadata={ + **previous.metadata, + "reference_bridge": { + "prefix_samples": count, + "control_dt": dt, + "joint_ids": ids, + }, + "source_collision_evidence": previous.metadata.get( + "collision_evidence", "unverified" + ), + "collision_evidence": "unverified", + "collision_scope": "not_checked", + }, + ) + + else: + previous = diagnostics or PlannerDiagnostics( + backend="reference_takeover" + ) + diagnostics = replace( + previous, + metadata={ + **previous.metadata, + "reference_bridge": { + "prefix_samples": 0, + "control_dt": dt, + "joint_ids": ids, + }, + }, + ) + # Composite skills may rebuild positions/timing after MotionGenerator. # Derive references only after that final composition, before lowering. velocities = timed.velocities @@ -1064,6 +1273,40 @@ def build_plan( velocities[:, 1:] = velocities[:, 1:].masked_fill(stationary, 0.0) velocities[:, 0] = 0.0 velocities[:, -1] = 0.0 + if diagnostics is not None and diagnostics.metadata.get("reference_bridge"): + join = int(diagnostics.metadata["reference_bridge"]["prefix_samples"]) + velocities[:, join] = 0.0 + ids = list(diagnostics.metadata["reference_bridge"]["joint_ids"]) + window_velocity = velocities[:, : join + 3, ids] + window_acceleration = ( + torch.diff(window_velocity, dim=1) + / timed.dt[:, 1 : window_velocity.shape[1], None] + ) + cap = self.robot.get_qvel_limits( + joint_ids=ids, env_ids=context.env_ids + ).to(window_velocity) + if request.motion_policy.velocity_limit is not None: + cap = cap.clamp(max=request.motion_policy.velocity_limit) + if not bool(torch.isfinite(cap[success_mask]).all()) or bool( + (cap[success_mask] <= 0).any() + ): + raise ValueError( + "Reference takeover requires finite positive velocity bounds" + ) + if bool( + ( + window_velocity[success_mask].abs() + > cap[success_mask, None] + 1e-5 + ).any() + ) or bool( + ( + window_acceleration[success_mask].abs() + > request.motion_policy.acceleration_limit + 1e-5 + ).any() + ): + raise ValueError( + "Reference takeover command derivatives exceed motion bounds" + ) timed = replace(timed, velocities=velocities) commands = self._joint_command_sequence( diff --git a/embodichain/lab/sim/atomic_skills/engine.py b/embodichain/lab/sim/atomic_skills/engine.py index 88392f6cb..1dfea346e 100644 --- a/embodichain/lab/sim/atomic_skills/engine.py +++ b/embodichain/lab/sim/atomic_skills/engine.py @@ -1453,6 +1453,15 @@ def rebuild_plan_from_trajectory( original.dt, ): raise ValueError("same-grid plan rebuilding requires identical dt.") + bridge = owned_plan.diagnostics.metadata.get("reference_bridge") + if bridge: + protected = int(bridge["prefix_samples"]) + 3 + if not torch.equal( + trajectory.positions[:, :protected], original.positions[:, :protected] + ): + raise ValueError( + "Plan rebuilding cannot change the reference takeover or its join stencil" + ) action = self._skills.get(request.skill_id) if action is None: raise KeyError( @@ -1618,6 +1627,15 @@ def rebuild_plan_from_mapped_trajectory( device, dtype = original.positions.device, original.positions.dtype candidate_positions = candidate.positions.to(device=device, dtype=dtype) candidate_dt = candidate.dt.to(original.dt) + bridge = owned.diagnostics.metadata.get("reference_bridge") + if bridge: + protected = int(bridge["prefix_samples"]) + 3 + if not torch.equal( + candidate_positions[:protected], original.positions[0, :protected] + ) or not torch.equal(candidate_dt[:protected], original.dt[0, :protected]): + raise ValueError( + "Retiming cannot alter the reference takeover or its join stencil" + ) if not torch.allclose( candidate_dt[1:], torch.full_like(candidate_dt[1:], context.require_control_dt()), diff --git a/embodichain/lab/sim/atomic_skills/execution.py b/embodichain/lab/sim/atomic_skills/execution.py index 4744c9cc4..87f1b6233 100644 --- a/embodichain/lab/sim/atomic_skills/execution.py +++ b/embodichain/lab/sim/atomic_skills/execution.py @@ -77,6 +77,7 @@ class ExecutionEventKind(str, Enum): REPLANNED = "replanned" TRACKING_DIVERGED = "tracking_diverged" TRACKING_FEEDBACK_FAILED = "tracking_feedback_failed" + BOUNDARY_DIAGNOSTIC = "boundary_diagnostic" TERMINAL_ACCEPTANCE_PENDING = "terminal_acceptance_pending" TERMINAL_ACCEPTANCE_FAILED = "terminal_acceptance_failed" DYNAMIC_GOAL_CHANGED = "dynamic_goal_changed" @@ -424,6 +425,10 @@ def __init__( ) self._terminal_started_at: float | None = None self._terminal_pending_reported = False + self._boundary_samples = 0 + self._boundary_stable = None + self._boundary_observed_at = None + self._boundary_position = None self._eligible = ( torch.ones_like(self._last_command_mask) if eligible_mask is None @@ -1222,6 +1227,115 @@ def tick( f"Unsupported terminal acceptance {type(terminal).__name__}." ) + # A normal task handoff may retain a tracking residual even though its + # nominal endpoint velocity is zero. Keep the exact validated terminal + # command (including grasp targets) until the selected motion joints settle. + boundary = plan.tracking_policy + ids = ( + [ + i + for endpoint in self._requests[self._invocation_index].binding.endpoints + if endpoint.endpoint_id == "motion" + for i in endpoint.joint_ids + ] + if boundary.boundary_stop + else [] + ) + ids = list(dict.fromkeys(ids)) + # Non-motion commands retain their existing terminal/effect semantics. + if ids and plan.joint_trajectory is not None: + import json + + target = plan.joint_trajectory.positions[:, -1, ids] + measured = self._context.robot.qpos[:, ids] + velocity = self._context.robot.qvel[:, ids] + error = (target - measured).abs().amax(dim=1) + native_speed = velocity.abs().amax(dim=1) + speed = native_speed + timestamp = self._context.robot.timestamp + if ( + self._boundary_position is not None + and timestamp > self._boundary_observed_at + ): + speed = ( + ( + (measured - self._boundary_position) + / (timestamp - self._boundary_observed_at) + ) + .abs() + .amax(dim=1) + ) + elif self._boundary_position is None: + speed = torch.full_like(native_speed, float("inf")) + finite = torch.isfinite(error) & torch.isfinite(native_speed) + ready = ( + finite + & (error <= boundary.boundary_position_tolerance) + & (speed <= boundary.boundary_velocity_tolerance) + ) + if self._boundary_stable is None: + self._boundary_stable = torch.zeros_like(error, dtype=torch.long) + if self._boundary_observed_at != self._context.robot.timestamp: + self._boundary_stable = torch.where( + ready & execution_mask, self._boundary_stable + 1, 0 + ) + self._boundary_observed_at = self._context.robot.timestamp + self._boundary_position = measured.clone() + pending = execution_mask & (self._boundary_stable < 2) + timed_out = pending & (elapsed_terminal >= boundary.boundary_timeout) + invalid = execution_mask & ~finite + if ( + self._boundary_samples == 0 + or not pending.any() + or timed_out.any() + or invalid.any() + ): + events.append( + self._event( + ExecutionEventKind.BOUNDARY_DIAGNOSTIC, + execution_mask, + json.dumps( + { + "joint_ids": ids, + "elapsed_s": elapsed_terminal, + "samples": self._boundary_samples, + "position_error": [ + v if math.isfinite(v) else None + for v in error.detach().cpu().tolist() + ], + "measured_speed": [ + v if math.isfinite(v) else None + for v in speed.detach().cpu().tolist() + ], + "native_speed": [ + v if math.isfinite(v) else None + for v in native_speed.detach().cpu().tolist() + ], + "velocity_source": "control_boundary_position_difference", + "position_tolerance": boundary.boundary_position_tolerance, + "velocity_tolerance": boundary.boundary_velocity_tolerance, + "ready": ready.detach().cpu().tolist(), + "timeout": timed_out.detach().cpu().tolist(), + }, + allow_nan=False, + ), + ) + ) + self._boundary_samples += 1 + if (timed_out | invalid).any(): + events.extend( + self._fail_tracking_feedback( + timed_out | invalid, + "Boundary takeover did not settle within its bounded timeout.", + ) + ) + if self._status is not ExecutionStatus.RUNNING: + return self._tick_result(command=None, events=events) + execution_mask = self._pending & plan.plan_success + pending &= execution_mask + terminal_pending &= execution_mask + terminal_pending |= pending + if terminal_pending.any(): if not self._terminal_pending_reported: events.append( @@ -1376,6 +1490,10 @@ def _install_plan( self._last_tracking_frame = None self._last_command_mask.zero_() self._tracking_violation_counts.zero_() + self._boundary_samples = 0 + self._boundary_stable = None + self._boundary_observed_at = None + self._boundary_position = None self._terminal_acceptance_counts.zero_() self._terminal_started_at = None self._terminal_pending_reported = False diff --git a/embodichain/lab/sim/atomic_skills/primitives/contact.py b/embodichain/lab/sim/atomic_skills/primitives/contact.py index 50db11855..6a37a5c54 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/contact.py +++ b/embodichain/lab/sim/atomic_skills/primitives/contact.py @@ -408,21 +408,20 @@ def _plan_contact_phase( active_mask=valid, ) success &= valid - checkpoints = [] - count = positions.shape[1] + checkpoint_targets = {} for post in phase.after: + segment_name = f"{post.kind}:{post.target}:{len(segments)}" if post.kind == "hold": n = source.hold_steps(post.target) else: - checkpoints.append((count - 1, post.target)) + checkpoint_targets[segment_name] = post.target n = 2 if type(n) is not int or n < 1: raise ValueError( "Contact settling targets must supply positive integer steps." ) positions = torch.cat((positions, positions[:, -1:].expand(-1, n, -1)), 1) - segments[f"{post.kind}:{post.target}:{len(segments)}"] = n - count += n + segments[segment_name] = n plan = action.build_plan( request, context, @@ -431,6 +430,12 @@ def _plan_contact_phase( segment_lengths=segments, replannable=False, ) + # A controller-reference prefix can shift the final command indices. + checkpoints = tuple( + (segment.start - 1, checkpoint_targets[segment.name]) + for segment in plan.segments + if segment.name in checkpoint_targets + ) return replace( plan, continuation=ContactContinuation( @@ -439,7 +444,7 @@ def _plan_contact_phase( phase.target, phase, resolved, - tuple(checkpoints), + checkpoints, ), ) diff --git a/embodichain/lab/sim/atomic_skills/state.py b/embodichain/lab/sim/atomic_skills/state.py index 0bc42d333..b297340a3 100644 --- a/embodichain/lab/sim/atomic_skills/state.py +++ b/embodichain/lab/sim/atomic_skills/state.py @@ -540,7 +540,13 @@ def exclusive_held_object_mask(self, resource: str) -> torch.Tensor: @dataclass(frozen=True, slots=True, eq=False) class RobotObservation: - """Measured robot state used as the start of planning or replanning.""" + """Measured robot state used as the start of planning or replanning. + + Optional ``command_qpos`` and ``command_qvel`` are independently owned + snapshots of controller references at the same control boundary. They are + not physical measurements and never replace ``qpos`` or ``qvel``. Projection + advances available references to the projected terminal position and rest. + """ timestamp: float qpos: torch.Tensor @@ -548,6 +554,8 @@ class RobotObservation: qeffort: torch.Tensor | None = None root_pose: torch.Tensor | None = None root_twist: torch.Tensor | None = None + command_qpos: torch.Tensor | None = None + command_qvel: torch.Tensor | None = None def __post_init__(self) -> None: if self.timestamp < 0.0: @@ -601,6 +609,20 @@ def __post_init__(self) -> None: ) if not torch.isfinite(self.root_twist).all(): raise ValueError("RobotObservation.root_twist must be finite.") + for name in ("command_qpos", "command_qvel"): + value = getattr(self, name) + if value is not None: + if not isinstance(value, torch.Tensor): + raise TypeError(f"{name} must be a torch.Tensor or None") + if ( + value.shape != self.qpos.shape + or value.device != self.qpos.device + or not torch.isfinite(value).all() + ): + raise ValueError( + f"{name} must be a finite aligned command snapshot" + ) + object.__setattr__(self, name, value.clone()) object.__setattr__(self, "qpos", self.qpos.clone()) object.__setattr__(self, "qvel", self.qvel.clone()) if self.qeffort is not None: @@ -636,6 +658,10 @@ def with_qpos(self, qpos: torch.Tensor) -> RobotObservation: qeffort=self.qeffort, root_pose=self.root_pose, root_twist=self.root_twist, + command_qpos=qpos if self.command_qpos is not None else None, + command_qvel=( + torch.zeros_like(qpos) if self.command_qvel is not None else None + ), ) diff --git a/embodichain/lab/sim/atomic_skills/tracking.py b/embodichain/lab/sim/atomic_skills/tracking.py index 0ef43f061..16b44572f 100644 --- a/embodichain/lab/sim/atomic_skills/tracking.py +++ b/embodichain/lab/sim/atomic_skills/tracking.py @@ -17,6 +17,7 @@ """Typed, transport-neutral tracking contracts for atomic-skill execution.""" from __future__ import annotations +import math from abc import ABC, abstractmethod from copy import deepcopy @@ -485,12 +486,54 @@ def snapshot(self) -> TimedTerminalAcceptance: @dataclass(frozen=True, slots=True) class TrackingPolicy: - """Independent in-flight recovery signal and terminal acceptance contract.""" + """Independent in-flight recovery signal and terminal acceptance contract. + + Args: + in_flight: Existing motion tracking and recovery policy. + terminal: Existing timed or feedback acceptance, retained unchanged. + boundary_reference_bridge: For supported native free entry phases, plan + a bounded controller-reference prefix before the original samples. + Requires independently captured position/velocity targets; external + nominal sources and undeclared phases retain their existing behavior. + boundary_stop: Additionally qualify explicitly bound motion joints before + completing an invocation. Hand endpoints retain their last commands. + boundary_position_tolerance: Maximum terminal reference error in joint + coordinates (radians for revolute, metres for prismatic joints). + boundary_velocity_tolerance: Maximum measured control-grid position + difference per second. Native velocity is separately checked finite. + boundary_timeout: Maximum physical seconds for boundary qualification. + Two distinct stable observations are required; failure is row-local. + + This is a tolerance-bounded stop, not a C1/C2 blending guarantee. It never + replaces original terminal, contact or effect gates. + """ in_flight: InFlightTrackingPolicy | None terminal: TerminalAcceptance + boundary_stop: bool = False + boundary_position_tolerance: float = 0.01 + boundary_velocity_tolerance: float = 0.005 + boundary_timeout: float = 1.5 + boundary_reference_bridge: bool = False def __post_init__(self) -> None: + if ( + type(self.boundary_reference_bridge) is not bool + or type(self.boundary_stop) is not bool + ): + raise TypeError("boundary_stop must be a bool") + for name in ( + "boundary_position_tolerance", + "boundary_velocity_tolerance", + "boundary_timeout", + ): + value = getattr(self, name) + if ( + type(value) not in (int, float) + or not math.isfinite(value) + or value <= 0 + ): + raise ValueError(f"{name} must be finite and positive") if self.in_flight is not None and not isinstance( self.in_flight, InFlightTrackingPolicy ): @@ -523,7 +566,15 @@ def __post_init__(self) -> None: ) def snapshot(self) -> TrackingPolicy: - return TrackingPolicy(self.in_flight, self.terminal) + return TrackingPolicy( + in_flight=self.in_flight, + terminal=self.terminal, + boundary_stop=self.boundary_stop, + boundary_reference_bridge=self.boundary_reference_bridge, + boundary_position_tolerance=self.boundary_position_tolerance, + boundary_velocity_tolerance=self.boundary_velocity_tolerance, + boundary_timeout=self.boundary_timeout, + ) @classmethod def timed(cls, *, settle_duration: float = 0.0) -> TrackingPolicy: diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 4fac890ac..56c9dc868 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -101,6 +101,9 @@ set_physical_visible, ) from embodichain.lab.sim.objects.backends.base import ArticulationViewBase +from embodichain.lab.sim.objects.backends._numeric_observations import ( + scoped_link_matrices, +) from embodichain.lab.sim.objects.backends.newton import ( _configure_newton_mimic_compliance, _stabilize_newton_mimic_target_write, @@ -1753,6 +1756,10 @@ def get_link_pose( ) link_idx = self.link_names.index(link_name) + if to_matrix: + selected = scoped_link_matrices(self, link_name, local_env_ids) + if selected is not None: + return selected link_pose = self.body_data.body_link_pose[local_env_ids, link_idx, :] if to_matrix: diff --git a/embodichain/lab/sim/objects/backends/_numeric_observations.py b/embodichain/lab/sim/objects/backends/_numeric_observations.py index 2c9e3ab47..43bd7d287 100644 --- a/embodichain/lab/sim/objects/backends/_numeric_observations.py +++ b/embodichain/lab/sim/objects/backends/_numeric_observations.py @@ -15,7 +15,7 @@ # ---------------------------------------------------------------------------- """Bounded shared native reads during a single World-owner planning boundary. -Only Default CPU full-batch numeric reads are shared. Each consumer still +Only Default CPU numeric reads are shared. Each consumer still receives its own output buffer. Checked native writes invalidate all samples; physics/topology revisions and scope exit prevent reuse across boundaries. Raw SDK mutations and concurrent World writers are outside this internal @@ -30,10 +30,12 @@ from threading import get_ident from typing import Any, Iterator, TYPE_CHECKING import torch +import numpy as np if TYPE_CHECKING: from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.objects.robot import Robot + from embodichain.lab.sim.objects.articulation import Articulation __all__: list[str] = [] _READS = frozenset( @@ -103,6 +105,62 @@ def key( return (id(batch), method, out.shape, out.dtype) +def scoped_link_matrices( + articulation: Articulation, link_name: str, rows: object +) -> torch.Tensor | None: + """Read a named link in the selected arenas during a Default CPU gather.""" + reads = _ACTIVE.get() + if reads is None or not reads.refresh(): + return None + view = articulation.body_data.articulation_view + batch = getattr(view, "batch", None) + if id(batch) not in reads.batches: + return None + if isinstance(rows, torch.Tensor): + if ( + rows.device.type != "cpu" + or rows.ndim != 1 + or rows.dtype not in (torch.int32, torch.int64) + ): + return None + rows = rows.tolist() + if ( + not isinstance(rows, (tuple, list)) + or not rows + or any( + type(row) is not int or not 0 <= row < len(articulation._entities) + for row in rows + ) + ): + return None + + from scipy.spatial.transform import Rotation + from embodichain.utils.math import matrix_from_quat + + matrices = [] + for row in rows: + key = (id(batch), "link_matrix", row, link_name) + cached = reads.values.get(key) + if cached is None: + handle = articulation._entities[row] + root = view.scene.get_arena(handle.arena_name).get_root_node() + arena = np.asarray(root.get_world_pose(), dtype=np.float32).reshape(4, 4) + world = np.asarray( + handle.get_link_pose(link_name), dtype=np.float32 + ).reshape(4, 4) + local = np.linalg.inv(arena) @ world + # Match the native CPU batch's float32 xyzw round-trip, including + # orthogonalization of slightly non-orthogonal physics rotations. + quat = Rotation.from_matrix(local[:3, :3]).as_quat().astype(np.float32) + matrix = torch.eye(4, device="cpu", dtype=torch.float32) + matrix[:3, :3] = matrix_from_quat(torch.from_numpy(quat)) + matrix[:3, 3] = torch.from_numpy(local[:3, 3]) + cached = (matrix, None) + reads.values[key] = cached + matrices.append(cached[0]) + return torch.stack(matrices) + + def shared_pose( batch: object, to_matrix: bool, read: Callable[[], torch.Tensor] ) -> torch.Tensor: diff --git a/embodichain/lab/task_program/integrations/_native_plan_expansion.py b/embodichain/lab/task_program/integrations/_native_plan_expansion.py index 66e3a72e2..641189ea2 100644 --- a/embodichain/lab/task_program/integrations/_native_plan_expansion.py +++ b/embodichain/lab/task_program/integrations/_native_plan_expansion.py @@ -26,6 +26,36 @@ __all__: list[str] = [] +def _fit_joint_residual( + reference, candidate, *, active, joint_limits, minimum_delta, checks +): + """Bound a sampled residual without changing time, endpoints or validators. + + Try the original draw first, then at most eight halvings. No new random + draws or smaller-than-declared variants are admitted. Each materialized + candidate is checked in its actual storage dtype before it can leave here. + """ + delta = candidate.positions - reference.positions + for refinement in range(9): + scale = 0.5**refinement + fitted = ( + replace(candidate) + if refinement == 0 + else replace(candidate, positions=reference.positions + scale * delta) + ) + magnitude = float((fitted.positions - reference.positions).abs().max()) + if magnitude < minimum_delta: + break + values = fitted.positions[:, active] + bounds = joint_limits[active] + if bool(((values < bounds[:, 0]) | (values > bounds[:, 1])).any()): + continue + result = checks(fitted) + if all(check.accepted for check in result): + return fitted, result, scale + return None + + @dataclass(frozen=True) class TaskProgramNativeExpansionRecord: """Owned measured-plan expansion provenance and hashed numeric attachment.""" @@ -60,6 +90,8 @@ def __init__( revision: str, output: Path | None = None, episode_id: int = 0, + affordance_branch: int | None = None, + preferred_local_poses: tuple[tuple[str, tuple[float, ...]], ...] = (), ): if operator not in ( "object_pose", @@ -77,6 +109,18 @@ def __init__( self.scene_case, self.revision = scene_case, revision self.output = output self.episode_id = episode_id + if affordance_branch is not None and ( + operator != "affordance" + or type(affordance_branch) is not int + or not 1 <= affordance_branch < self.options["affordance"]["branches"] + ): + raise ValueError( + "affordance_branch must select a non-nominal configured branch" + ) + self.affordance_branch = affordance_branch + if preferred_local_poses and operator != "affordance": + raise ValueError("preferred_local_poses requires the affordance operator") + self.preferred_local_poses = preferred_local_poses self._records: list[TaskProgramNativeExpansionRecord] = [] @property @@ -126,7 +170,12 @@ def prepare_planning_context(self, request, context, *, engine): count=settings["branches"], seed=self.seed, episode_id=self.episode_id, - branch_override=1 + self.episode_id % (settings["branches"] - 1), + preferred_local_poses=self.preferred_local_poses, + branch_override=( + self.affordance_branch + if self.affordance_branch is not None + else 1 + self.episode_id % (settings["branches"] - 1) + ), ), ) @@ -285,35 +334,72 @@ def motion_checks(trajectory): f"{reference_checks}" ) reason = "no distinct candidate" - for attempt in range(self.options["max_proposals"]): - arguments = dict( - joint_limits=limits, - normalized_scale=self.options["joint_offset_scale"], - generator=generator, - ) - try: - if self.operator == "joint_residual": - candidate = joint_residual(template, **arguments) - elif self.operator == "via_points": - candidate = via_points( - template, via_count=self.options["via_count"], **arguments - ) - elif self.operator == "nullspace_residual": - candidate = nullspace_residual( - template, task_jacobians=jacobians, **arguments - ) - else: - raise ValueError(f"Unsupported operator: {self.operator}") - except ProposalRejected as exc: - reason = str(exc) - continue - delta = float((candidate.positions - template.positions).abs().max()) - if delta < self.options["minimum_joint_delta"]: - continue - checks = motion_checks(candidate) - if any(not check.accepted for check in checks): - reason = f"edited-phase motion limits: {checks}" - continue + best_rejected = None + can_scale_residual = self.operator in ("joint_residual", "via_points") + budget = self.options["max_proposals"] + for iteration in range(budget + 1): + residual_scale = 1.0 + if iteration == budget: + # Preserve the existing random draws and full-amplitude preference. + # Only budget exhaustion activates this bounded deterministic fallback. + if not can_scale_residual or best_rejected is None: + break + _, attempt, sampled, raw_checks = best_rejected + fitted = _fit_joint_residual( + template, + sampled, + active=active, + joint_limits=limits, + minimum_delta=self.options["minimum_joint_delta"], + checks=motion_checks, + ) + if fitted is None: + reason = f"best motion rejection at proposal {attempt}: {raw_checks}; bounded refinement cannot meet unchanged limits and minimum delta" + break + candidate, checks, residual_scale = fitted + sampled_positions = sampled.positions + delta = float((candidate.positions - template.positions).abs().max()) + else: + attempt = iteration + arguments = dict( + joint_limits=limits, + normalized_scale=self.options["joint_offset_scale"], + generator=generator, + ) + try: + if self.operator == "joint_residual": + candidate = joint_residual(template, **arguments) + elif self.operator == "via_points": + candidate = via_points( + template, via_count=self.options["via_count"], **arguments + ) + elif self.operator == "nullspace_residual": + candidate = nullspace_residual( + template, task_jacobians=jacobians, **arguments + ) + else: + raise ValueError(f"Unsupported operator: {self.operator}") + except ProposalRejected as exc: + reason = str(exc) + continue + delta = float((candidate.positions - template.positions).abs().max()) + if delta < self.options["minimum_joint_delta"]: + continue + sampled_positions = candidate.positions + checks = motion_checks(candidate) + raw_checks = checks + if any(not check.accepted for check in checks): + reason = f"edited-phase motion limits: {checks}" + if can_scale_residual: + score = max( + value + for report in checks + for check in report.checks + for value in check.metrics.values() + ) + if best_rejected is None or score < best_rejected[0]: + best_rejected = (score, attempt, candidate, checks) + continue fk = self._check_fk(template, candidate, kinematics) if self.operator == "nullspace_residual" and ( fk["max_translation_m"] @@ -340,6 +426,11 @@ def motion_checks(trajectory): "task_rows": self.options["nullspace_task_rows"], "max_joint_delta": delta, "proposal": attempt, + "residual_scale": residual_scale, + "raw_proposal_count": min(iteration + 1, budget), + "raw_motion_checks": [ + dict(c.metrics) for r in raw_checks for c in r.checks + ], "same_time_grid": True, "motion_validation_scope": "edited_phases_and_boundary_stencils", "motion": { @@ -370,6 +461,11 @@ def motion_checks(trajectory): candidate=candidate.positions.numpy(), dt=template.dt.numpy(), editable=editable.numpy(), + **( + {"raw_candidate": sampled_positions.numpy()} + if residual_scale < 1 + else {} + ), ) evidence["plan_artifact"] = { "path": str(path), diff --git a/embodichain/lab/task_program/integrations/catalog.py b/embodichain/lab/task_program/integrations/catalog.py index f93686b8d..4e48be6aa 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -236,6 +236,18 @@ def _canonical_value(value: object) -> object: and not value.supports_segment_motion_recovery ): metadata.pop("supports_segment_motion_recovery") + from embodichain.lab.sim.atomic_skills.tracking import TrackingPolicy + + if type(value) is TrackingPolicy and not value.boundary_reference_bridge: + metadata.pop("boundary_reference_bridge") + if type(value) is TrackingPolicy and not value.boundary_stop: + for key in ( + "boundary_stop", + "boundary_position_tolerance", + "boundary_velocity_tolerance", + "boundary_timeout", + ): + metadata.pop(key) if type(value) is MotionPolicy: if value.final_velocity_rejection == "hold": metadata.pop("final_velocity_rejection") diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 5ab4ec52c..b3fd489ad 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1961,6 +1961,27 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: def _decode_tracking_policy(value: object, *, path: str) -> TrackingPolicy: + from dataclasses import replace + + if not isinstance(value, Mapping): + return _decode_tracking_policy_base(value, path=path) + config = dict(value) + boundary = { + name: config.pop(name) + for name in tuple(config) + if name + in { + "boundary_reference_bridge", + "boundary_stop", + "boundary_position_tolerance", + "boundary_velocity_tolerance", + "boundary_timeout", + } + } + return replace(_decode_tracking_policy_base(config, path=path), **boundary) + + +def _decode_tracking_policy_base(value: object, *, path: str) -> TrackingPolicy: """Decode one allowlisted tracking policy.""" common = _mapping( value, diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 7c6262930..c584779fe 100644 --- a/embodichain/lab/task_program/integrations/expansion.py +++ b/embodichain/lab/task_program/integrations/expansion.py @@ -207,6 +207,8 @@ def from_native_plan( revision: str, output: Path | None = None, episode_id: int = 0, + affordance_branch: int | None = None, + preferred_local_poses: tuple[tuple[str, tuple[float, ...]], ...] = (), ) -> TaskProgramSameGridExpansionFactory: """Expand live Atomic plans on their existing measured control grid. @@ -218,7 +220,10 @@ def from_native_plan( scene_case: Measured scene and initial-state identity. revision: Pinned task/source revision. output: Optional directory for hashed reference/candidate attachments. - episode_id: Logical affordance branch identity. + episode_id: Logical episode identity, also used for round-robin sampling. + affordance_branch: Optional non-nominal branch frozen in the motion recipe. + preferred_local_poses: Previously accepted local grasp matrices by + sampling key for selecting among the current valid candidates. This source path retains native operators and Atomic reconstruction, protects phase-boundary derivative stencils and never claims physical @@ -235,6 +240,8 @@ def from_native_plan( revision=revision, output=output, episode_id=episode_id, + affordance_branch=affordance_branch, + preferred_local_poses=preferred_local_poses, ) return instance diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 8c862f2e9..f180c88a2 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -517,6 +517,8 @@ class SimulationPlanningObservationProvider(GymPlanningObservationProvider): owner_token: Factory identity used to reject mixed runtime components. runtime_scope_guard: Optional host epoch/lifetime validator. It must read no robot/sensor state and must not advance simulation. + capture_command_targets: Capture owned controller targets for profiles + declaring reference-conditioned handoffs; otherwise avoid those reads. capture_root_pose: Capture the robot's arena-local root transform in each observation. The default leaves ``RobotObservation.root_pose`` unset and performs no root-pose query. @@ -534,12 +536,15 @@ def __init__( owner_token: object, runtime_scope_guard: Callable[[], None] | None = None, capture_root_pose: bool = False, + capture_command_targets: bool = False, hold_qpos_source: Literal["observed", "drive_target"] = "observed", ) -> None: if type(scene_provider) is not SharedTickSceneProvider: raise TypeError("scene_provider must be exactly SharedTickSceneProvider.") if type(clock) is not EnvironmentStepClock: raise TypeError("clock must be exactly EnvironmentStepClock.") + if type(capture_command_targets) is not bool: + raise TypeError("capture_command_targets must be a bool") if type(capture_root_pose) is not bool: raise TypeError("capture_root_pose must be a bool.") if hold_qpos_source not in ("observed", "drive_target"): @@ -576,6 +581,7 @@ def __init__( self._owner_token = owner_token self._validate_scope = validate_scope self._capture_root_pose = capture_root_pose + self._capture_command_targets = capture_command_targets self._hold_qpos_source = hold_qpos_source super().__init__(self._capture) @@ -708,6 +714,16 @@ def _capture(self, task_state: TaskState) -> PlanningContext: context = PlanningContext( robot=RobotObservation( timestamp=timestamp, + command_qpos=( + self._robot.get_qpos(target=True).index_select(0, self._env_ids) + if self._capture_command_targets + else None + ), + command_qvel=( + self._robot.get_qvel(target=True).index_select(0, self._env_ids) + if self._capture_command_targets + else None + ), qpos=qpos.index_select(0, self._env_ids), qvel=qvel.index_select(0, self._env_ids), qeffort=( @@ -1259,6 +1275,10 @@ def create_planning_observation_provider( owner_token=self._owner_token, runtime_scope_guard=runtime_scope_guard, capture_root_pose=self._capture_root_pose, + capture_command_targets=any( + preset.tracking_policy.boundary_reference_bridge + for preset in self._registration.catalog.robot_profile.presets.values() + ), hold_qpos_source=( "drive_target" if cable is not None else self._hold_qpos_source ), diff --git a/embodichain/lab/task_program/runtime/results.py b/embodichain/lab/task_program/runtime/results.py index 4851fc16b..0e6e5a577 100644 --- a/embodichain/lab/task_program/runtime/results.py +++ b/embodichain/lab/task_program/runtime/results.py @@ -483,7 +483,7 @@ def _tracking_policy_to_metadata(policy: TrackingPolicy) -> dict[str, object]: """Serialize independent in-flight and terminal tracking contracts.""" in_flight = policy.in_flight terminal = policy.terminal - return { + result = { "in_flight": ( None if in_flight is None @@ -511,6 +511,17 @@ def _tracking_policy_to_metadata(policy: TrackingPolicy) -> dict[str, object]: } ), } + if policy.boundary_stop: + result["boundary_stop"] = { + "position_tolerance": policy.boundary_position_tolerance, + "velocity_tolerance": policy.boundary_velocity_tolerance, + "timeout": policy.boundary_timeout, + "velocity_source": "control_boundary_position_difference", + "consecutive_observations": 2, + } + if policy.boundary_reference_bridge: + result["boundary_reference_bridge"] = True + return result def _tracking_sequence_to_metadata( diff --git a/examples/expansion/engine/assets/reference-handoff.png b/examples/expansion/engine/assets/reference-handoff.png new file mode 100644 index 000000000..a1b22289c Binary files /dev/null and b/examples/expansion/engine/assets/reference-handoff.png differ diff --git a/examples/expansion/engine/assets/residual-refinement.png b/examples/expansion/engine/assets/residual-refinement.png new file mode 100644 index 000000000..842ee00df Binary files /dev/null and b/examples/expansion/engine/assets/residual-refinement.png differ diff --git a/examples/expansion/engine/assets/residual-repair-handoff.png b/examples/expansion/engine/assets/residual-repair-handoff.png new file mode 100644 index 000000000..bf8047bfe Binary files /dev/null and b/examples/expansion/engine/assets/residual-repair-handoff.png differ diff --git a/examples/expansion/engine/assets/skill-boundaries.png b/examples/expansion/engine/assets/skill-boundaries.png new file mode 100644 index 000000000..d68bc62fd Binary files /dev/null and b/examples/expansion/engine/assets/skill-boundaries.png differ diff --git a/examples/expansion/engine/assets/via-points-handoff.png b/examples/expansion/engine/assets/via-points-handoff.png new file mode 100644 index 000000000..53ba730cc Binary files /dev/null and b/examples/expansion/engine/assets/via-points-handoff.png differ diff --git a/examples/expansion/engine/skill-boundaries.md b/examples/expansion/engine/skill-boundaries.md new file mode 100644 index 000000000..55e9fc886 --- /dev/null +++ b/examples/expansion/engine/skill-boundaries.md @@ -0,0 +1,313 @@ +# Skill boundary diagnosis and controlled stops + +Native configured expansion jobs enable `tracking.boundary_stop` in their owned +execution-policy snapshot. Authored policy files, physical/contact validators, +scene samplers and grasp/release gates are unchanged. Scalar component overrides +and inline policy declarations use the existing canonical resolver. + +```yaml +tracking: + kind: timed + boundary_stop: true + boundary_position_tolerance: 0.01 + boundary_velocity_tolerance: 0.005 + boundary_timeout: 1.5 +``` + +Explicit `false` retains historical behavior. Direct Atomic users remain opt-in. +Position tolerance uses each joint's coordinate units (rad or m); velocity uses +those units per second. Configure tighter/application-specific values when the +controller and physical task support them. Defaults accommodate the observed +steady reference error of the Default UR5 pick-place setup; they are not universal +robot safety limits or an acceleration/jerk guarantee. + +After the nominal final command, the session continues sending the same command +until two distinct measured control boundaries satisfy target-position error and +finite-difference position speed. Hand targets are retained; no trajectory sample +is modified. This reduces residual physical motion before the next skill captures +its actual start state. The remaining reference-to-measurement error can still +cause a bounded target jump. This feature does not implement fly-through blending. + +Native qvel is validated finite and logged separately. This matters for backends +whose post-substep velocity differs from control-grid displacement. Existing +terminal acceptance remains an additional requirement. Timeout and invalid data +reject only affected rows and preserve ordinary safe-stop behavior. New frames +consume wall/step budgets and are recorded as actual actions with T+1 observations. + +`boundary_diagnostic` runtime events contain the bound joint IDs, error, native +and control-grid measured speeds, elapsed time, readiness, timeout and tolerances. +The first control-speed sample is unknown and serialized as null. Failed and +cancelled runs must not be pooled into successful episode statistics. + +## Reproduce comparisons + +Use separate baseline and modified checkouts with the same Python environment, +CPU affinity and visible GPU. Run each pair serially in before/after and then +after/before order, keeping all settings identical: + +```bash +OMP_NUM_THREADS=1 MKL_NUM_THREADS=1 SDL_VIDEODRIVER=offscreen CUDA_VISIBLE_DEVICES=1 \ +python -m scripts.benchmark.expansion.task_pose_coverage \ + --worker --group A --scene open --rows 4 --rollouts 16 \ + --candidates 512 --seconds 600 --output /absolute/fresh-result +``` + +Repeat for group D. The task owns Default physics; `collision_backend: newton` +is the independent collision checker, not a switch of the physical backend. +Retain configuration, source manifest, attempts, rejected evidence and receipts. +The benchmark source manifest includes Atomic and Gym Task Program code. + +`scripts/benchmark/expansion/skill_boundaries.py` consumes a comparison directory +with `runs.json` and `{A,D}-{0,1}-{before,after}` worker directories. Each run entry +contains case, group, revision and metrics (wall_s, committed, executed). It outputs +per-boundary JSON, percentile summaries and six-joint position/velocity PNG/PDF: + +```bash +python -m scripts.benchmark.expansion.skill_boundaries --input /absolute/comparison +``` + +The plotted pair is chosen by largest baseline target jump among successful +same-ordinal pairs whose initial object poses match within 1e-4 componentwise. +The plot reports raw positions and control-grid finite differences, aligned to +first Place command, without postprocessing smoothing. Aggregate results include +all successful boundaries; success rates retain all executed attempts. The source +artifacts and pair selection are recorded for independent inspection. + +## Recorded Default-backend qualification + +Baseline: `fb96705b0`. UR5 pick-place, open scene, seed 7, B=4, 16 rollout +budget and 512 proposal budget, groups A (bounded/FIFO) and D +(workspace/coverage), each run serially in before/after then after/before order. +Physics is Default CPU; independent geometry checking uses Newton CPU. Visible +GPU 1 is held constant; eight CPU cores and one numerical thread are used. +Measured wall time includes setup, execution, original acceptance and export, +not Python process import. Full source/config manifests are retained per run. + +| Group | Confirmed / executed before → after | Mean wall seconds before → after | Boundary command jump P95 (rad) before → after | Measured control-speed P95 (rad/s) before → after | +| --- | --- | --- | --- | --- | +| A | 30/32 → 30/32 | 54.12 → 56.47 | 0.009904 → 0.009540 | 0.021962 → 0.003043 | +| D | 20/26 → 18/26 | 48.71 → 49.19 | 0.006324 → 0.005695 | 0.021708 → 0.003002 | + +D did not fill the requested quota in either version: proposal budgets were +exhausted. The new gate rejects one additional previously accepted sample per +repeat: terminal residual approximately 0.01066 rad exceeds the explicit 0.01 +threshold after 1.5 s, despite zero control-grid speed. No existing predicate +was relaxed. D's successful-set boundary statistics therefore have a different +population; they are not an isolated causal improvement estimate. Effective +accepted throughput decreases approximately 4.2% for A and 10.9% for D. Two +orders/one seed are insufficient for a general performance claim. + +The mechanism demonstrably reduces residual measured motion, but does not remove +reference jumps from steady tracking bias. It should not be advertised as a +complete cure for command discontinuities. A future boundary-conditioned planner +is required for strict continuous references. The evidence shown below deliberately +retains the visible command spikes. + + + +The pair is A, repeat 0, candidate ordinal 15: largest baseline target jump among +matching successful pairs, initial object pose components differ by at most +1.06e-6. Blue is before, orange after; solid is command, dashed measured. Speeds +are finite differences on the 40 ms control grid. Time zero is each recording's +first Place command; recorded signals are not smoothed. + +A collection driver is also available: + +```bash +CUDA_VISIBLE_DEVICES=1 python -m scripts.benchmark.expansion.skill_boundaries \ + --collect --baseline /absolute/baseline-checkout \ + --revision /absolute/modified-checkout --input /absolute/new-comparison +``` + +## Reference-conditioned takeover (follow-up) + +`tracking.boundary_reference_bridge` adds a rest-to-rest controller-reference +prefix for supported native free entry phases. Expansion inherits its default +from `boundary_stop`; an explicit stop opt-out therefore retains legacy behavior +unless the bridge is separately enabled. The Gym observation provider captures +owned position/velocity targets separately from measured qpos/qvel. Planning +never relabels those references as measurements. + +The prefix starts at the previous drive target with zero target velocity and ends +at the original first plan sample. Existing minimum-jerk interpolation provides +bounded samples on the original control grid. Position limits, discrete speed +and acceleration across the join, and velocity-target derivatives are checked. +The original suffix positions and velocity references remain intact, with the +original zero-velocity start enforced at the join. Prefix samples are real +commands, not a smoothing pass over exported evidence. + +The prefix inherits the first phase name so held-object and phase guards remain +active. Segment lengths and scene-monitor indices are shifted together. Expansion +and mapped retiming cannot overwrite the prefix or join stencil. Rebuilding cannot +prepend twice. Projected compilation advances reference snapshots consistently; +failed rows do not contribute a stale reference or moving target to bridge sizing. +Original collision evidence is invalidated for a changed plan; existing runtime +geometry/contact/task acceptance still owns qualification. No continuous collision +certificate is claimed. This initial bridge also requires revision zero and disabled action replanning/retry +budgets: zero feedforward velocity alone cannot prove a position-only reference +is stationary during an in-flight recovery. External nominal sources and undeclared entry phases keep +their original behavior and report the unavailable bridge in diagnostics. + +### Drive-bias diagnosis + +Four matched layouts retain bitwise-identical Pick terminal command targets: + +| Diagnostic-only configuration | Mean per-episode maximum arm-joint error | +| --- | --- | +| Original arm stiffness 10000, damping 1000 | 5.842 mrad | +| Stiffness 20000, damping 1000 | 2.900 mrad | +| Stiffness 10000, damping 500 | 4.387 mrad | + +These controlled interventions support gain-dependent steady tracking bias rather +than changed geometric endpoints. They are excluded from performance/success +comparisons. Native post-substep velocity differs from control-grid displacement; +the recorded effort channel is zero and is not treated as measured drive torque. +A gravity-toggle probe had no independently verified post-reset readback and is +inconclusive, so no claim attributes the entire bias to gravity or saturation. +The handoff planner removes the reference reset; it does not eliminate physical +tracking bias or retune the robot's gains. + +### Paired qualification against the stop-only version + +Baseline is `532fe6ef4`, with the same B=4, seed 7, 16-rollout/512-proposal budgets, +Default CPU physics and both serial run orders used above. Every successful +Pick→Place boundary has an exactly zero command position jump after the change. +The acceleration window is reported explicitly in each JSON row (normally +−0.04 to +0.48 s relative to handoff), so a shifted nearby spike is not hidden. + +| Group | Confirmed / executed before → after | Mean wall seconds | Jump P95 (mrad) | Window command acceleration P95 (rad/s²) | +| --- | --- | --- | --- | --- | +| A | 30/32 → 30/32 | 52.31 → 54.61 | 9.534 → 0 | 5.961 → 0.446 | +| D | 18/26 → 18/26 | 48.76 → 50.16 | 5.695 → 0 | 3.560 → 0.400 | + +Window maxima are 0.459 and 0.400 rad/s² after takeover, below the unchanged +0.5 bound. Wall-time increases are 4.4% and 2.9%. These are two repeats of one +seed, not a general performance guarantee. Existing D quota shortfalls and +stop-gate rejection remain; no gate was loosened to recover success. + +An initial joint-residual smoke was 4/4 before and 3/4 after: the original +edited-phase acceleration validator rejects one generated Place variant +(acceleration ratio 1.425). This failure is retained as evidence; the bounded repair below addresses this +budget-exhaustion case. Broader operator qualification remains separate. Main matrix results are object-pose +operator results, not a claim that all trajectory operators preserve yield. + + + +The paired plot uses ordinal 15 with exactly matching initial object poses. +Blue is stop-only; orange includes planned takeover. Solid lines are commands, +dashed lines measurements. Velocities are unsmoothed control-grid differences; +they are not native instantaneous velocity readings. The numerical matrix has +per-run source manifests; later fail-closed rebuild/retiming and interface guards +have focused tests plus a separate B=4 native smoke. + +## Joint-residual rejection recovery + +The earlier 3/4 probe exposed a near-limit rejection-sampling weakness, not a +failure of the reference prefix. The failure occurs in Place/retract candidate +checks, before plan restoration/rebuilding. The nominal phase already consumes +about 99% of the acceleration bound. Residual amplitude is sampled from joint +range, independently of the phase's remaining dynamic margin. A fixed random +budget can therefore miss a feasible amplitude; preserving endpoints alone does +not constrain discrete acceleration. The old error reports the last rejection, +not the closest one. + +The native adapter now preserves the original random sequence and preference +for unscaled candidates. Only after raw `max_proposals` is exhausted does it +retain the closest motion rejection and try at most eight deterministic +halvings. Every materialized result must pass the unchanged joint/motion checks +and original minimum delta, followed by unchanged FK, reconstruction, collision +and physical/task validation. It does not retime, alter protected samples or +accept nominal repeats. Memory remains O(TD), retaining one candidate rather +than the entire proposal pool. This is bounded local repair, not a complete +feasibility solver. + +Provenance includes `proposal` (raw source index), `raw_proposal_count`, +`residual_scale`, raw checks and the accepted checks. Refined plan archives also +include `raw_candidate`; original passing draws retain their amplitude. + +### Same-configuration qualification + +Baseline `6cd7da36b`; Default CPU, group A, joint_residual, seed 7, B=4, +4-rollout/128-scene-candidate budgets, 64 raw motion proposals, 240 seconds. +Velocity/acceleration limits remain 0.5; minimum joint delta remains 0.002 rad. +Both before/after and after/before were run serially with the same CPU affinity +and visible GPU. Earlier exploratory baseline runs also achieved 4/4: the +failure is sensitive to the small dynamic margin, not a guaranteed failure on +every run of the current baseline. + +| Metric | Before | After | +| --- | --- | --- | +| Confirmed/executed, two orders | 6/8 (3/4 each) | 8/8 (4/4 each) | +| Mean wall seconds, setup through export | 21.776 | 21.208 | +| Qualified episodes/minute | 8.27 | 11.32 | +| Pick→Place command jump, maximum | 0 rad | 0 rad | +| Handoff-window command acceleration, maximum | 0.415 rad/s² | 0.411 rad/s² | + +The restored ordinal 2 exhausts all 64 raw draws. Its closest draw (index 4) +has acceleration ratio 1.000464, above the unchanged 1+1e-6 acceptance threshold. +A residual scale of 0.5 yields ratio 0.993311 (0.993013 on the second repeat) +and actual max delta 0.009999 rad, above the original 0.002 minimum. All endpoints, +hand columns and noneditable samples match the reference exactly. The original +reference itself still must pass validation; repair cannot bypass that gate. + + + +The handoff plot compares shared successful ordinal 1. The failed ordinal 2 had +no executed Place path before repair; its raw rejected plan is shown separately: + + + +The 2.6% wall-time difference is descriptive for this small sample, not a general +speedup claim. Effective throughput improves mainly by recovering the fourth +qualified episode. The rejected candidate is not labeled as measured execution. +Analysis supports `--operator joint_residual`; per-run source manifests and raw +artifacts retain the comparison evidence. Final changes to archive size and +private ownership were followed by a separate native smoke. + +## Via-point rejection recovery + +The native `via_points` adapter uses the same bounded fallback after exhausting +the original full-amplitude proposal budget. It ranks the rejected motion +candidates by their worst limit ratio, then tries at most eight halvings on +one candidate. Each candidate must retain the original minimum perturbation +and pass the original joint, speed and acceleration checks. Shape scalings use +the same control grid and preserve the two protected samples at each editable +phase edge. Accepted candidates follow the ordinary FK, reconstruction and +measured collision and task acceptance. Provenance retains the raw checks and +final scale for every accepted proposal, with a separate raw path for a refinement. + +On the original UR5 open pick-place configuration (Default CPU, B=4, four +rollouts, 128 scene candidates, 64 motion proposals per eligible call), the +five-seed baseline scan confirmed 18/20. Seed 27 had one Pick plan failure and +one Place stop-gate timeout. A fixed-knot regression with one raw proposal +reproduces motion-limit rejection at 2.898 times the acceleration cap. A +quarter-scale refinement passes the same cap, preserves release and retract +edge frames, and retains the original time grid and stationary joint commands. +The one-proposal fixture exercises the fallback independently of the native +benchmark's unchanged 64-proposal budget. + +| Same-configuration comparison | Confirmed / executed | End-to-end wall time | Pick-to-Place command jump | +| --- | --- | --- | --- | +| Seed 7, three serial pairs including reversed order | 12/12 → 12/12 | Median 22.93 → 23.09 s | 0 → 0 rad | +| Seed 27, one serial pair | 2/4 → 2/4 | 23.13 → 23.31 s | 0 → 0 rad | + +Every accepted native via-point draw used scale 1. Both versions retain the two +seed-27 rejections and all confirmed episodes pass `path_collision`, `lift` and +`placed_and_released`. Seed 7 wall times were 22.92, 22.93, 23.57 s before and +22.17, 37.17, 23.09 s after. The 37.17 s run spent 14.78 s between environment +initialization and the solver limit message, versus 4.53–4.76 s in the other +first four runs. Keep this run in the results and interpret the timing as a +shared-host sample. All pairs use CPU affinity 24–31, visible GPU 3, and one +numerical thread. The run configuration and original 0.5 rad/s, 0.5 rad/s², +and 2 mrad bounds agree across versions. + + + +The figure compares command and measured joint motion around the first Place +command at seed 7, ordinal 2, on the same initial layout. Both curves retain +the planned reference takeover. Seed 7's peak command acceleration within the +handoff window is 0.411 → 0.412 rad/s², below the original 0.5 cap. + +Use the existing worker with `--operator via_points --seed 7 --rows 4 +--rollouts 4 --candidates 128 --seconds 240` for this workload. The boundary +reporter accepts `--operator via_points` for the resulting episode archives. diff --git a/scripts/benchmark/expansion/skill_boundaries.py b/scripts/benchmark/expansion/skill_boundaries.py new file mode 100644 index 000000000..095278a35 --- /dev/null +++ b/scripts/benchmark/expansion/skill_boundaries.py @@ -0,0 +1,470 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Analyze real pick/place handoffs and plot paired measured/command signals. + +Run: python -m scripts.benchmark.expansion.skill_boundaries --input COMPARISON_DIR +Input contains runs.json and A/D-repeat-before/after native benchmark outputs. +The collection command and fixed CPU/GPU settings are retained in each run log. +""" + +from __future__ import annotations +import argparse +import json +import os +import subprocess +import sys +import time +import hashlib +from pathlib import Path +import numpy as np + +__all__: list[str] = [] + + +def _episodes(directory: Path, operator: str = "object_pose") -> list[dict]: + result = [] + for path in sorted( + (directory / f"collection/dataset/{operator}/episodes").glob("*/episode.npz") + ): + with np.load(path, allow_pickle=False) as arrays: + m = json.loads(arrays["manifest"].tobytes()) + meta = m["metadata"] + names = meta["expert_action_spec"]["joint_names"] + # This acceptance task explicitly controls the first six UR5 arm joints. + q = arrays[ + "observation_" + str(m["observations"].index("full_joint_positions")) + ][:, :6] + a = arrays["actions"][:, :6] + t = arrays["timestamps"] + for segment in meta["program_segments"][1:]: + k = segment["start_step"] + if not 2 <= k < len(a) - 1: + continue + dt = np.diff(t) + command_speed = np.diff(a, axis=0) / dt[:-1, None] + measured_speed = np.diff(q, axis=0) / dt[:, None] + attempts = [ + attempt + for call in segment.get("runtime", {}).get("calls", ()) + for attempt in call.get("plan_attempts", ()) + ] + prefix = next( + ( + int( + attempt.get("planner_diagnostics", {}) + .get("metadata", {}) + .get("reference_bridge", {}) + .get("prefix_samples", 0) + ) + for attempt in reversed(attempts) + ), + 0, + ) + row = { + "takeover_duration_s": prefix * float(dt[k]), + "episode": m["episode_id"], + "ordinal": meta["expansion_recipe"]["ordinal"], + "boundary": segment["name"], + "step": k, + "duration_s": float(t[-1] - t[0]), + "steps": len(a), + "joint_names": names[:6], + "acceleration_window_s": [ + float(t[max(0, k - 3) + 2] - t[k]), + float(t[min(len(command_speed), k + 12)] - t[k]), + ], + "window_measured_acceleration_rad_s2": float( + np.abs( + np.diff( + measured_speed[ + max(0, k - 3) : min(len(measured_speed), k + 12) + ], + axis=0, + ) + / float(dt[k]) + ).max() + ), + "window_command_acceleration_rad_s2": float( + np.abs( + np.diff( + command_speed[ + max(0, k - 3) : min(len(command_speed), k + 12) + ], + axis=0, + ) + / float(dt[k]) + ).max() + ), + "command_jump_rad": float(np.abs(a[k] - a[k - 1]).max()), + "command_difference_speed_rad_s": float( + np.abs(command_speed[k - 1]).max() + ), + "command_difference_acceleration_rad_s2": float( + np.abs( + (command_speed[k - 1] - command_speed[k - 2]) / dt[k] + ).max() + ), + "measured_control_speed_rad_s": float( + np.abs(measured_speed[k - 1]).max() + ), + "initial_objects": { + v["uid"]: v["pose"] + for v in meta["initial_state"] + if v["kind"] == "rigid_object" + }, + "artifact": str(path), + } + ix = np.arange(max(1, k - 15), min(len(a) - 1, k + 16)) + row["curve"] = { + "time_s": (t[ix] - t[k]).tolist(), + "command_q": a[ix].tolist(), + "measured_q": q[ix].tolist(), + "command_difference_velocity": command_speed[ix - 1].tolist(), + "measured_difference_velocity": measured_speed[ix - 1].tolist(), + } + result.append(row) + return result + + +def _plot(before: dict, after: dict, output: Path) -> None: + import matplotlib + + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + fig, axes = plt.subplots(6, 2, figsize=(12, 15), sharex=True) + colors = {"before": "#3267a8", "after": "#df7c28"} + for label, row in (("before", before), ("after", after)): + c = row["curve"] + t = c["time_s"] + for j in range(6): + for col, keys in enumerate( + ( + ("command_q", "measured_q"), + ("command_difference_velocity", "measured_difference_velocity"), + ) + ): + for key, style in zip(keys, ("-", "--")): + axes[j, col].plot( + t, + np.asarray(c[key])[:, j], + style, + color=colors[label], + lw=1.5, + label=label + (" command" if style == "-" else " measured"), + ) + for j in range(6): + for col in range(2): + if after.get("takeover_duration_s", 0): + axes[j, col].axvspan( + 0, + after["takeover_duration_s"], + color="#777777", + alpha=0.09, + label="planned takeover" if j == 0 and col == 0 else None, + ) + axes[j, col].axvline(0, color="#555", lw=0.8) + axes[j, col].grid(alpha=0.2) + axes[j, col].set_ylabel( + before["joint_names"][j] + (" [rad]" if col == 0 else " [rad/s]") + ) + axes[0, 0].set_title("Joint position: command and measured") + axes[0, 1].set_title("Velocity: control-grid finite differences") + axes[0, 0].legend(fontsize=8) + axes[0, 1].legend(fontsize=8) + for ax in axes[-1]: + ax.set_xlabel("Seconds relative to first Place command") + fig.suptitle( + "Same proposed layout, ordinal " + + str(before["ordinal"]) + + "; no smoothing of recorded signals", + fontsize=13, + ) + fig.tight_layout(rect=(0, 0, 1, 0.97)) + fig.savefig(output / "joint-boundaries.png", dpi=160) + fig.savefig(output / "joint-boundaries.pdf") + plt.close(fig) + + +def _collect( + root: Path, baseline: Path, revision: Path, operator: str = "object_pose" +) -> None: + if root.exists(): + raise FileExistsError("Collection requires a fresh output directory") + root.mkdir(parents=True) + cpus = sorted(os.sched_getaffinity(0))[:8] + environment = dict( + os.environ, + OMP_NUM_THREADS="1", + MKL_NUM_THREADS="1", + SDL_VIDEODRIVER="offscreen", + ) + runs = [] + for group in ("A", "D"): + for repeat, order in enumerate((("before", "after"), ("after", "before"))): + for label in order: + workspace = baseline if label == "before" else revision + case = f"{group}-{repeat}-{label}" + command = [ + "taskset", + "-c", + ",".join(map(str, cpus)), + sys.executable, + "-m", + "scripts.benchmark.expansion.task_pose_coverage", + "--worker", + "--group", + group, + "--operator", + operator, + "--scene", + "open", + "--rows", + "4", + "--rollouts", + "16", + "--candidates", + "512", + "--seconds", + "600", + "--output", + str(root / case), + ] + start = time.perf_counter() + with (root / f"{case}.log").open("w") as log: + process = subprocess.run( + command, + cwd=workspace, + env=environment, + stdout=log, + stderr=subprocess.STDOUT, + ) + metrics = json.loads((root / case / "metrics.json").read_text()) + files = ( + "embodichain/lab/sim/atomic_skills/execution.py", + "embodichain/lab/sim/atomic_skills/tracking.py", + "embodichain/lab/expansion/task_program_jobs.py", + "embodichain/lab/task_program/integrations/configured.py", + "embodichain/lab/task_program/integrations/_native_plan_expansion.py", + ) + runs.append( + { + "case": case, + "group": group, + "repeat": repeat, + "revision": label, + "returncode": process.returncode, + "command": command, + "affinity": cpus, + "cold_process_s": time.perf_counter() - start, + "source": { + f: hashlib.sha256((workspace / f).read_bytes()).hexdigest() + for f in files + }, + "metrics": { + k: metrics[k] + for k in ( + "wall_s", + "committed", + "executed", + "physical_accepted", + "error", + "peak_rss_mib", + ) + }, + } + ) + (root / "runs.json").write_text(json.dumps(runs, indent=2)) + print(case, runs[-1]["metrics"], flush=True) + if process.returncode or metrics["error"]: + raise RuntimeError(f"Benchmark worker failed: {case}") + + +def _main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input", type=Path, required=True) + parser.add_argument("--collect", action="store_true") + parser.add_argument( + "--operator", + choices=("object_pose", "joint_residual", "via_points"), + default="object_pose", + ) + parser.add_argument("--baseline", type=Path) + parser.add_argument( + "--revision", type=Path, default=Path(__file__).resolve().parents[3] + ) + args = parser.parse_args() + root = args.input.resolve() + if args.collect: + if args.baseline is None: + parser.error("--collect requires --baseline") + _collect(root, args.baseline.resolve(), args.revision.resolve(), args.operator) + runs = json.loads((root / "runs.json").read_text()) + rows = [] + by_case = {} + for run in runs: + data = _episodes(root / run["case"], args.operator) + if len({row["episode"] for row in data}) != run["metrics"]["committed"]: + raise ValueError( + f"Boundary records do not match confirmed episode count: {run['case']}" + ) + by_case[run["case"]] = data + for row in data: + rows.append( + { + **row, + "case": run["case"], + "group": run["group"], + "revision": run["revision"], + } + ) + summaries = [] + for group in ("A", "D"): + for revision in ("before", "after"): + selected = [ + r for r in rows if r["group"] == group and r["revision"] == revision + ] + matching = [ + r for r in runs if r["group"] == group and r["revision"] == revision + ] + if not matching: + continue + summary = { + "group": group, + "revision": revision, + "confirmed": sum(r["metrics"]["committed"] for r in matching), + "executed": sum(r["metrics"]["executed"] for r in matching), + "wall_s_runs": [r["metrics"]["wall_s"] for r in matching], + "boundary_count": len(selected), + } + for metric in ( + "window_measured_acceleration_rad_s2", + "window_command_acceleration_rad_s2", + "command_jump_rad", + "command_difference_speed_rad_s", + "command_difference_acceleration_rad_s2", + "measured_control_speed_rad_s", + "duration_s", + "steps", + ): + summary[metric + "_p50_p95_max"] = ( + np.quantile([r[metric] for r in selected], [0.5, 0.95, 1]).tolist() + if selected + else None + ) + summaries.append(summary) + pairs = [] + b = by_case.get("A-0-before", []) + a = {r["ordinal"]: r for r in by_case.get("A-0-after", [])} + for row in b: + other = a.get(row["ordinal"]) + if other is None: + continue + error = max( + np.max( + np.abs(np.asarray(value) - np.asarray(other["initial_objects"][uid])) + ) + for uid, value in row["initial_objects"].items() + ) + if error <= 1e-4: + pairs.append((row, other, float(error))) + plot = None + if pairs: + before, after, error = max(pairs, key=lambda pair: pair[0]["command_jump_rad"]) + _plot(before, after, root) + before_zero = int(np.argmin(np.abs(before["curve"]["time_s"]))) + after_zero = int(np.argmin(np.abs(after["curve"]["time_s"]))) + plot = { + "handoff_measured_q_difference_max_rad": float( + np.max( + np.abs( + np.asarray(before["curve"]["measured_q"][before_zero]) + - after["curve"]["measured_q"][after_zero] + ) + ) + ), + "preceding_reference_difference_max_rad": float( + np.max( + np.abs( + np.asarray(before["curve"]["command_q"][before_zero - 1]) + - after["curve"]["command_q"][after_zero - 1] + ) + ) + ), + "selection": "largest before command jump among successful same-ordinal pairs with initial object poses within 1e-4", + "ordinal": before["ordinal"], + "max_initial_pose_component_error": error, + "before": before["artifact"], + "after": after["artifact"], + } + report = { + "schema_version": 1, + "operator": args.operator, + "summary": summaries, + "plot_pair": plot, + "rows": rows, + "limitations": [ + "Accepted-episode boundary metrics exclude failed attempts; success denominator includes all executed attempts.", + "Velocity and acceleration here are control-grid finite differences, not native physics readouts.", + "Two orders, one seed, same host: latency is descriptive, not a general speedup claim.", + ], + } + (root / "boundary-report.json").write_text( + json.dumps(report, indent=2, allow_nan=False) + ) + import html + import statistics + + lines = [] + for value in summaries: + jump = value["command_jump_rad_p50_p95_max"] + acceleration = value["window_command_acceleration_rad_s2_p50_p95_max"] + lines.append( + "
'
+ if plot
+ else "没有满足配对条件的轨迹。
" + ) + (root / "report.html").write_text( + '' + "固定配置,正反运行顺序。速度和加速度是控制网格差分;统计仅覆盖确认成功的边界,成功率分母保留全部执行尝试。蓝色为实现前、橙色为实现后;实线为命令,虚线为实测。
" + '| 采样 | 版本 | 成功/执行 | 平均耗时 s | 跳变 P95 mrad | 窗口加速度 P95 rad/s² |
|---|
初始布局匹配的代表轨迹,未对原始数据做平滑。具体窗口和配对规则见 JSON。
" + + figure + + '' + ) + print(json.dumps(summaries, indent=2)) + + +if __name__ == "__main__": + _main() diff --git a/scripts/benchmark/expansion/task_pose_coverage.py b/scripts/benchmark/expansion/task_pose_coverage.py index 154f43886..9837548cc 100644 --- a/scripts/benchmark/expansion/task_pose_coverage.py +++ b/scripts/benchmark/expansion/task_pose_coverage.py @@ -115,6 +115,8 @@ def _worker(args) -> None: if path.startswith( ( "embodichain/lab/expansion/", + "embodichain/lab/sim/atomic_skills/", + "embodichain/lab/gym/envs/task_program/", "embodichain/lab/task_program/", "extensions/embodichain_tasks/configs/tasks/manipulation/workspace_pose_pick_place/", "scripts/benchmark/expansion/task_pose_coverage.py", @@ -387,7 +389,9 @@ def main() -> None: p.add_argument("--nominal", action="store_true") p.add_argument("--media", action="store_true") p.add_argument( - "--operator", choices=("object_pose", "joint_residual"), default="object_pose" + "--operator", + choices=("object_pose", "joint_residual", "via_points"), + default="object_pose", ) args = p.parse_args() args.output = args.output.resolve() @@ -494,6 +498,7 @@ def main() -> None: ( "joint_residual" if args.suite == "trajectory" + and args.operator == "object_pose" else args.operator ), ] diff --git a/tests/benchmark/expansion/test_task_pose_report.py b/tests/benchmark/expansion/test_task_pose_report.py index 46ef649d1..4a410c3aa 100644 --- a/tests/benchmark/expansion/test_task_pose_report.py +++ b/tests/benchmark/expansion/test_task_pose_report.py @@ -76,3 +76,36 @@ def test_formal_gate_rejects_incomplete_or_incompatible_records(tmp_path, change json.loads((tmp_path / "acceptance.json").read_text())["open"]["status"] == "unqualified" ) + + +def test_boundary_report_derivatives_use_recorded_control_grid(tmp_path): + import json + import numpy as np + from scripts.benchmark.expansion.skill_boundaries import _episodes + + folder = tmp_path / "collection/dataset/object_pose/episodes/test" + folder.mkdir(parents=True) + manifest = { + "episode_id": "test", + "observations": ["full_joint_positions"], + "metadata": { + "expert_action_spec": {"joint_names": [f"joint{i}" for i in range(6)]}, + "program_segments": [{"start_step": 0}, {"start_step": 2, "name": "place"}], + "expansion_recipe": {"ordinal": 0}, + "initial_state": [], + }, + } + np.savez( + folder / "episode.npz", + manifest=np.frombuffer(json.dumps(manifest).encode(), dtype=np.uint8), + timestamps=np.arange(5) * 0.04, + actions=np.repeat(np.array([0, 0.1, 0.15, 0.2])[:, None], 6, axis=1), + observation_0=np.repeat( + np.array([0, 0.01, 0.03, 0.06, 0.10])[:, None], 6, axis=1 + ), + ) + row = _episodes(tmp_path)[0] + assert row["command_jump_rad"] == pytest.approx(0.05) + assert row["command_difference_speed_rad_s"] == pytest.approx(1.25) + assert row["command_difference_acceleration_rad_s2"] == pytest.approx(31.25) + assert row["measured_control_speed_rad_s"] == pytest.approx(0.5) diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 0836d645a..5d973ff57 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -5423,3 +5423,21 @@ def test_inspector_json_stdout_contains_only_the_result(tmp_path: Path) -> None: assert data["deployments"][0]["resolved_configuration"]["components"][ "integration" ].endswith("integration.tabletop.yaml") + + +def test_boundary_policy_decoding_keeps_existing_tracking_requirements(): + from embodichain.lab.task_program.integrations.configured import ( + _decode_tracking_policy, + ) + + raw = { + "kind": "timed", + "settle_duration": 0.4, + "boundary_stop": True, + "boundary_timeout": 2.0, + } + policy = _decode_tracking_policy(raw, path="tracking") + assert policy.boundary_stop and policy.boundary_timeout == 2.0 + assert policy.terminal.settle_duration == 0.4 + assert raw["boundary_stop"] is True + assert not _decode_tracking_policy({"kind": "timed"}, path="tracking").boundary_stop diff --git a/tests/lab/expansion/test_configured.py b/tests/lab/expansion/test_configured.py index 1da22d262..53cfe2fa3 100644 --- a/tests/lab/expansion/test_configured.py +++ b/tests/lab/expansion/test_configured.py @@ -179,3 +179,54 @@ def test_deployment_snapshot_preserves_component_relative_assets(tmp_path, owner resolved = resolve_gym_components(snapshot, base_dir=derived.parent).config assert resolved["rigid_object"][0]["shape"]["fpath"] == str(mesh) assert {p.name: p.read_bytes() for p in source.iterdir()} == before + + +@pytest.mark.parametrize("selector", ["path", "component", "inline"]) +def test_expansion_boundary_policy_preserves_selector_and_original_file( + tmp_path, selector +): + import json + from pathlib import Path + import yaml + from embodichain.lab.expansion.task_program_jobs import _prepare_deployment + + root = Path(__file__).resolve().parents[3] + source = ( + root + / "extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop.yaml" + ) + policy = yaml.safe_load(source.read_text()) + owned = tmp_path / "policy.yaml" + owned.write_text(yaml.safe_dump(policy)) + before = owned.read_bytes() + selection = str(owned) + if selector == "component": + selection = { + "component": str(owned), + "overrides": {"motion": {"sample_count": 23}}, + } + if selector == "inline": + selection = {"inline": policy} + env = tmp_path / "env.yaml" + env.write_text("physics: default\n") + deployment = tmp_path / "task.yaml" + deployment.write_text( + yaml.safe_dump( + { + "id": "test", + "environment": {"component": str(env)}, + "task_program": {"execution_policy": selection}, + } + ) + ) + output = tmp_path / "out" + output.mkdir() + prepared = json.loads(_prepare_deployment(deployment, output).read_text()) + result = yaml.safe_load( + Path(prepared["task_program"]["execution_policy"]).read_text() + ) + assert result["tracking"]["boundary_stop"] is True + assert result["motion"]["sample_count"] == ( + 23 if selector == "component" else policy["motion"]["sample_count"] + ) + assert owned.read_bytes() == before diff --git a/tests/lab/expansion/test_recipes.py b/tests/lab/expansion/test_recipes.py index 07d7844cd..246a1886b 100644 --- a/tests/lab/expansion/test_recipes.py +++ b/tests/lab/expansion/test_recipes.py @@ -17,10 +17,107 @@ """Logical randomness is invariant to batching and unrelated operator fields.""" from __future__ import annotations +import json +from dataclasses import dataclass +from types import SimpleNamespace import pytest from embodichain.lab.expansion import PinnedExpertSource, UniformRangeOperator +def test_affordance_feedback_freezes_choices_and_keeps_exploring(tmp_path): + from embodichain.lab.expansion.recipes import ( + BatchCompatibility, + ExpansionRecipe, + OperatorCapability, + RecipeOperation, + ) + from embodichain.lab.expansion.task_program_jobs import _task_expansion_extensions + + source = PinnedExpertSource("task_program", "v1") + world = BatchCompatibility( + "robot", ("j0",), "topology", "program", "default", 0.04, "obs", "world" + ) + operation = RecipeOperation( + "motion", "v1", OperatorCapability("motion", "plan", "environment"), "{}" + ) + options = { + "trajectory": { + "affordance": { + "branches": 4, + "call_indices": [0], + "branch_selection": "outcome_guided", + } + } + } + ports = _task_expansion_extensions( + options=options, operator="affordance", seed=1, report={"recipe_revision": "v1"} + ) + + def recipe(ordinal): + return ports.recipe_transform( + ExpansionRecipe(source.context(1, ordinal), world, (operation,)) + ) + + def branch(recipe): + return json.loads(recipe.operations[0].payload_json)["affordance_branch"] + + def outcome(segments, samples=None): + return SimpleNamespace( + segments=([{"success": True}] * segments,), + expansion_records=(({"samples": samples or {}},),), + ) + + first = [recipe(i) for i in range(3)] + assert [branch(value) for value in first] == [1, 2, 3] + ports.plan_outcome(2, False, outcome(1)) + ports.plan_outcome(0, False, outcome(1)) + ports.plan_outcome(1, False, outcome(3)) + assert [recipe(i) for i in range(3)] == first + guided = recipe(3) + assert branch(guided) == 2 + import numpy as np + + reference = np.eye(4) + reference[0, 3] = 2 + grasp = reference.copy() + grasp[0, 3] += 0.125 + sample = { + "key": "pick:grasp", + "pose_frame": "world", + "success": [True], + "reference_poses": [reference.tolist()], + "selected_poses": [grasp.tolist()], + } + ports.plan_outcome(3, True, outcome(4, {"grasp": sample})) + ports.plan_outcome( + 3, False, outcome(0) + ) # A repeated notification cannot revoke the result. + retry = recipe(4) + assert branch(retry) == 2 + prior = json.loads(retry.operations[0].payload_json)["accepted_grasps"] + assert prior["ordinal"] == 3 and prior["local_poses"]["pick:grasp"][3] == 0.125 + recipe(5) + assert "accepted_grasps" not in json.loads(recipe(6).operations[0].payload_json) + assert {branch(recipe(i)) for i in range(4, 17)} == {1, 2, 3} + + @dataclass + class Context: + batch_size: int = 1 + affordance_sampling: object = None + + row = SimpleNamespace(ordinal=4, directory=tmp_path, recipe=retry, scene_case=None) + factory = ports.plan_transform_factory(None, row) + prepared = factory.prepare_planning_context( + SimpleNamespace(analysis_call_index=0), Context(), engine=None + ) + assert prepared.affordance_sampling.episode_id == 4 + assert prepared.affordance_sampling.branch_override == 2 + assert ( + dict(prepared.affordance_sampling.preferred_local_poses)["pick:grasp"][3] + == 0.125 + ) + + @pytest.mark.parametrize("batch", [1, 8, 32]) def test_random_streams_are_slot_and_completion_order_independent(batch): source = PinnedExpertSource("task_program", "source-v1") diff --git a/tests/lab/expansion/test_residual_refinement.py b/tests/lab/expansion/test_residual_refinement.py new file mode 100644 index 000000000..74d0aafd8 --- /dev/null +++ b/tests/lab/expansion/test_residual_refinement.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations +from dataclasses import replace +import torch +from embodichain.lab.expansion.contracts import TrajectoryTemplate +from embodichain.lab.sim.motion.expansion.operators import validate_motion_samples +from embodichain.lab.task_program.integrations._native_plan_expansion import ( + _fit_joint_residual, +) + + +def test_refinement_obeys_original_limits_and_keeps_owned_endpoints(): + reference = TrajectoryTemplate( + "s", + "r", + "t", + ("j",), + torch.zeros(5, 1), + torch.tensor([0.0, 1.0, 1.0, 1.0, 1.0]), + ) + candidate = replace( + reference, positions=torch.tensor([[0.0], [0.0], [1.0], [0.0], [0.0]]) + ) + + def check(value): + return [ + validate_motion_samples( + value.positions, + value.dt, + velocity_limits=torch.tensor([1.0]), + acceleration_limits=torch.tensor([0.5]), + ) + ] + + assert not check(candidate)[0].accepted + fitted, checks, scale = _fit_joint_residual( + reference, + candidate, + active=[0], + joint_limits=torch.tensor([[-2.0, 2.0]]), + minimum_delta=0.1, + checks=check, + ) + assert scale == 0.25 + assert fitted.positions[2, 0] == 0.25 and all(c.accepted for c in checks) + assert torch.equal( + fitted.positions[[0, 1, 3, 4]], reference.positions[[0, 1, 3, 4]] + ) + fitted.positions.zero_() + assert candidate.positions[2, 0] == 1.0 + assert ( + _fit_joint_residual( + reference, + candidate, + active=[0], + joint_limits=torch.tensor([[-2.0, 2.0]]), + minimum_delta=0.3, + checks=check, + ) + is None + ) + + +def test_valid_draw_retains_original_amplitude(): + reference = TrajectoryTemplate( + "s", "r", "t", ("j",), torch.zeros(3, 1), torch.tensor([0.0, 1.0, 1.0]) + ) + candidate = replace(reference, positions=torch.tensor([[0.0], [0.1], [0.0]])) + + def check(value): + return [ + validate_motion_samples( + value.positions, + value.dt, + velocity_limits=torch.tensor([1.0]), + acceleration_limits=torch.tensor([0.5]), + ) + ] + + fitted, _, scale = _fit_joint_residual( + reference, + candidate, + active=[0], + joint_limits=torch.tensor([[-1.0, 1.0]]), + minimum_delta=0.01, + checks=check, + ) + assert scale == 1 and torch.equal(fitted.positions, candidate.positions) + fitted.positions.zero_() + assert candidate.positions[1, 0] == 0.1 diff --git a/tests/lab/task_program/test_expansion.py b/tests/lab/task_program/test_expansion.py index dbfca8f63..3523ebe8a 100644 --- a/tests/lab/task_program/test_expansion.py +++ b/tests/lab/task_program/test_expansion.py @@ -1106,3 +1106,79 @@ class CustomMotion(_ThreePhasePlaceAction): == factory.records[0].trajectory_selected == 1 ) + + +def test_native_via_point_refines_after_the_raw_proposal_budget( + monkeypatch: pytest.MonkeyPatch, +) -> None: + from embodichain.lab.sim.motion.expansion import operators + from embodichain.lab.task_program.integrations._native_plan_expansion import ( + _NativePlanExpansion, + ) + + def plan_place(action, request, context): + positions = torch.zeros((1, 26, 3)) + positions[:, 4:, 2] = 0.02 + positions[:, 6:, 0] = torch.linspace(0, 0.19, 20) + trajectory = TimedTrajectory.from_uniform_step( + positions, env_ids=context.env_ids, step_dt=CONTROL_DT + ) + return action.build_plan( + request, + context, + success=True, + trajectory=trajectory, + segment_lengths={"approach": 3, "release": 3, "retract": 20}, + ) + + monkeypatch.setattr(_ThreePhasePlaceAction, "_plan", plan_place) + engine, invocation, resolved, context, plan = _planned_place() + engine.robot.get_qpos_limits.return_value = torch.tensor([[-2.0, 2.0]] * 3) + engine.robot.get_qvel_limits.return_value = torch.full((3,), 0.5) + monkeypatch.setattr(_NativePlanExpansion, "_kinematics", lambda *_: (None, ())) + sample_via_points = operators.via_points + raw_proposals = [] + + def fixed_knot(template, **kwargs): + candidate = sample_via_points( + template, joint_samples={"j0": 1.0}, knot_samples=(1.0,), **kwargs + ) + raw_proposals.append(candidate) + return candidate + + monkeypatch.setattr(operators, "via_points", fixed_knot) + factory = TaskProgramSameGridExpansionFactory.from_native_plan( + "via_points", + { + "free_phases": {"place": ["retract"]}, + "velocity_limit": 0.5, + "acceleration_limit": 0.5, + "max_proposals": 1, + "minimum_joint_delta": 0.002, + "joint_offset_scale": 0.05, + "via_count": 1, + "nullspace_task_rows": [0], + }, + seed=7, + scene_case=SceneCase( + "case", "state", "scene", "repeated_cube_pick_place", "robot" + ), + revision="reference", + ) + + transform = factory.create_plan_transform(_plan_request(invocation), engine=engine) + rebuilt = transform(resolved, context, plan) + evidence = rebuilt.diagnostics.metadata["expansion"] + assert len(raw_proposals) == evidence["raw_proposal_count"] == 1 + assert evidence["raw_motion_checks"][0]["acceleration_ratio"] > 1 + assert evidence["motion"]["candidate"][0]["acceleration_ratio"] <= 1 + assert evidence["residual_scale"] == 0.25 + assert evidence["max_joint_delta"] >= 0.002 + assert rebuilt.diagnostics.metadata["collision_evidence"] == "unverified" + assert rebuilt.segments == plan.segments + assert torch.equal(rebuilt.joint_trajectory.dt, plan.joint_trajectory.dt) + original = plan.joint_trajectory.positions + positions = rebuilt.joint_trajectory.positions + assert torch.equal(positions[:, :8], original[:, :8]) + assert torch.equal(positions[:, -2:], original[:, -2:]) + assert torch.equal(positions[:, :, 1:], original[:, :, 1:]) diff --git a/tests/sim/atomic_skills/test_affordance_sampling.py b/tests/sim/atomic_skills/test_affordance_sampling.py index 79e1cde8f..2dc029818 100644 --- a/tests/sim/atomic_skills/test_affordance_sampling.py +++ b/tests/sim/atomic_skills/test_affordance_sampling.py @@ -65,6 +65,44 @@ def test_sampling_context_rejects_invalid_stream_identity(field: str, value: obj AffordanceSamplingContext(**values) +def test_preferred_grasp_requires_a_current_valid_nonnominal_local_pose(): + ref = torch.eye(4)[None] + ref[:, 0, 3] = 2.0 + poses = ref[:, None].repeat(1, 3, 1, 1) + poses[0, :, 0, 3] += torch.tensor([0.0, 0.125, 0.5]) + costs = torch.arange(3.0)[None] + valid = torch.ones((1, 3), dtype=torch.bool) + + def sample(preferred_x, mask, branch=1): + desired = torch.eye(4) + desired[0, 3] = preferred_x + ctx = AffordanceSamplingContext( + count=3, + branch_override=branch, + preferred_local_poses=(("pick", tuple(desired.flatten().tolist())),), + ) + return sampling_module._sample_pose_candidates( + AffordancePoseCandidates(poses, costs, mask), + sampling=ctx, + key="pick", + env_ids=torch.tensor([0]), + reference_poses=ref, + ).metadata + + matched = sample(0.125, valid) + assert matched["matched_preferred_local_pose"] == [True] + assert matched["candidate_ids"] == [1] + for preferred_x, mask in ( + (0.0, valid), + (0.2, valid), + (0.125, torch.tensor([[True, False, True]])), + ): + ranked = sample(preferred_x, mask) + assert ranked["matched_preferred_local_pose"] == [False] + assert ranked["candidate_ids"] == [2] + assert sample(0.125, valid, branch=0)["candidate_ids"] == [0] + + def test_sampling_context_uses_private_reproducible_generators(): context = AffordanceSamplingContext(count=4, seed=3, episode_id=2, attempt_id=1) global_state = torch.random.get_rng_state().clone() diff --git a/tests/sim/atomic_skills/test_contact_paths.py b/tests/sim/atomic_skills/test_contact_paths.py index 1068d856f..32c6cdc65 100644 --- a/tests/sim/atomic_skills/test_contact_paths.py +++ b/tests/sim/atomic_skills/test_contact_paths.py @@ -27,11 +27,13 @@ PickUpOptions, MotionPolicy, RecoveryPolicy, + TrackingPolicy, ) from embodichain.lab.sim.atomic_skills.primitives.contact import ( ContactPlaceGoal, ContactPickUpGoal, ContactPhase, + ContactPostcondition, ResolvedContactPhase, ) from embodichain.lab.sim.atomic_skills.state import RobotObservation @@ -341,6 +343,47 @@ def test_contact_pickup_plans_pose_close_and_lift_without_rigid_projection(): assert pose_plan.commands.frame_count == 3 +def test_contact_checkpoint_follows_lift_after_reference_bridge(): + action = _bind_action(_motion_generator(), PickUp()) + engine = _ACTION_ENGINES[id(action)] + source = _GeometrySource() + engine.planning_services.register_contact_source("geometry", source) + phase = ContactPhase("grasp", "pinch", (ContactPostcondition("check", "held"),)) + invocation = replace( + _invocation(action, ContactPickUpGoal("geometry", (phase,))), + motion_policy=MotionPolicy( + strategy="ik_interp", dynamic_collision_mode="off", acceleration_limit=2.0 + ), + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + tracking_policy=replace(TrackingPolicy.timed(), boundary_reference_bridge=True), + skill_options=PickUpOptions(pre_grasp_distance=0, lift_height=0), + ) + context = _context() + arm = invocation.binding.endpoint("primary", "motion").target.joint_ids + reference = context.robot.qpos.clone() + reference[:, arm[0]] += 0.001 # A stationary reference offset requires a prefix. + context = replace( + context, + robot=replace( + context.robot, + command_qpos=reference, + command_qvel=torch.zeros_like(reference), + ), + ) + velocity_limits = engine.robot.get_qvel_limits.side_effect + engine.robot.get_qvel_limits.side_effect = ( + lambda env_ids=None, **kw: velocity_limits(**kw) + ) + engine.robot.get_qpos_limits.return_value = torch.tensor([-2.0, 2.0]).repeat( + context.batch_size, len(arm), 1 + ) + + plan = engine.plan(invocation, context) + assert plan.diagnostics.metadata["reference_bridge"]["prefix_samples"] > 0 + lift = next(segment for segment in plan.segments if segment.name == "lift") + assert plan.continuation.checkpoints == ((lift.stop - 1, "held"),) + + def test_velocity_qualification_also_rejects_an_unsafe_continued_grasp(monkeypatch): source = _GeometrySource(closed=1.0) action = _bind_action(_motion_generator(), PickUp()) diff --git a/tests/sim/atomic_skills/test_engine_per_env.py b/tests/sim/atomic_skills/test_engine_per_env.py index b998412b0..ba2515b06 100644 --- a/tests/sim/atomic_skills/test_engine_per_env.py +++ b/tests/sim/atomic_skills/test_engine_per_env.py @@ -3528,3 +3528,232 @@ def test_failed_effect_plan_retries_without_requesting_effect_verification() -> assert any( event.kind is ExecutionEventKind.RECOVERY_EXHAUSTED for event in failed.events ) + + +def test_boundary_stop_keeps_terminal_target_until_distinct_stable_observations(): + engine, _ = _engine() + invocation = replace( + _invocation(engine), + tracking_policy=replace(TrackingPolicy.timed(), boundary_stop=True), + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.0, 0.0, 0.2, 0)) + session.tick(_context(0.1, 0.0, 0.2, 0)) + waiting = session.tick(_context(0.2, 0.19, 0.2, 0)) + assert torch.allclose(_joint_positions(waiting.command), torch.full((1, 2), 0.2)) + ready = _context(0.3, 0.2, 0.2, 0) + assert session.tick(ready).status is ExecutionStatus.RUNNING + assert session.tick(ready).status is ExecutionStatus.RUNNING + assert session.tick(_context(0.4, 0.2, 0.2, 0)).status is ExecutionStatus.RUNNING + assert session.tick(_context(0.5, 0.2, 0.2, 0)).status is ExecutionStatus.COMPLETED + + +def test_boundary_timeout_does_not_keep_failed_rows_in_timed_hold(): + engine, _ = _engine(batch_size=2) + invocation = replace( + _invocation(engine), + tracking_policy=replace( + TrackingPolicy.timed(settle_duration=2), + boundary_stop=True, + boundary_timeout=0.3, + ), + ) + session = engine.start((invocation,), _context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.2, (0.0, 0.2), (0.2, 0.2), 0)) + session.tick(_context(0.3, (0.0, 0.2), (0.2, 0.2), 0)) + result = session.tick(_context(0.5, (0.0, 0.2), (0.2, 0.2), 0)) + assert result.eligible_mask.tolist() == [False, True] + assert result.command.active_mask.tolist() == [False, True] + + +def test_nonfinite_boundary_speed_rejects_only_affected_row(): + engine, _ = _engine(batch_size=2) + invocation = replace( + _invocation(engine), + tracking_policy=replace(TrackingPolicy.timed(), boundary_stop=True), + ) + session = engine.start((invocation,), _context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.0, (0.0, 0.0), (0.2, 0.2), 0)) + session.tick(_context(0.1, (0.0, 0.0), (0.2, 0.2), 0)) + context = _context(0.2, (0.2, 0.2), (0.2, 0.2), 0) + context = replace( + context, + robot=replace( + context.robot, qvel=torch.tensor([[float("nan"), 0.0], [0.0, 0.0]]) + ), + ) + result = session.tick(context) + assert result.eligible_mask.tolist() == [False, True] + assert result.command.active_mask.tolist() == [False, True] + + +def test_boundary_stop_leaves_non_motion_command_contract_unchanged(): + engine, _ = _destination_engine(destinations=("first",)) + invocation = replace( + _destination_invocation(engine), + tracking_policy=replace(TrackingPolicy.timed(), boundary_stop=True), + ) + session = engine.start((invocation,), _context(0.0, 0.0, 0.2, 0)) + assert session.tick(_context(0.0, 0.0, 0.2, 0)).command is not None + assert session.tick(_context(0.1, 0.0, 0.2, 0)).status is ExecutionStatus.COMPLETED + + +def test_reference_bridge_preserves_phase_and_rebuild_join(monkeypatch): + from embodichain.lab.sim.atomic_skills._expansion_phases import FREE_PHASES + + engine, action = _engine(batch_size=2) + monkeypatch.setitem(FREE_PHASES, "dynamic", frozenset({"approach"})) + engine.robot.get_qvel_limits.return_value = torch.full((2, 2), 0.5) + engine.robot.get_qpos_limits.return_value = torch.tensor( + [[[-3.0, 3.0], [-3.0, 3.0]]] * 2 + ) + + def plan(request, context): + points = torch.stack( + ( + context.robot.qpos, + context.robot.qpos + 0.00005, + context.robot.qpos + 0.0001, + ), + dim=1, + ) + return action.build_plan( + request, + context, + success=torch.tensor([True, False]), + trajectory=TimedTrajectory.from_uniform_step( + points, env_ids=context.env_ids, step_dt=0.1 + ), + segment_lengths={"approach": 3}, + ) + + monkeypatch.setattr(action, "_plan", plan) + context = _context(0.0, (0.0, 0.0), (0.0001, 0.0001), 0, control_dt=0.1) + reference = torch.tensor([[0.01, 0.01], [100.0, 100.0]]) + context = replace( + context, + robot=replace( + context.robot, + command_qpos=reference, + command_qvel=torch.tensor([[0.0, 0.0], [10.0, 10.0]]), + ), + ) + invocation = replace( + _invocation(engine), + tracking_policy=replace(TrackingPolicy.timed(), boundary_reference_bridge=True), + ) + invocation = replace( + invocation, + recovery_policy=replace( + invocation.recovery_policy, max_replans=0, max_skill_retries=0 + ), + ) + result = engine.plan(invocation, context) + join = result.diagnostics.metadata["reference_bridge"]["prefix_samples"] + assert result.segments[0].name == "approach" + assert torch.equal(result.joint_trajectory.positions[0, 0], reference[0]) + assert torch.equal( + result.joint_trajectory.positions[0, join], context.robot.qpos[0] + ) + assert torch.equal(result.joint_trajectory.velocities[:, join], torch.zeros((2, 2))) + resolved = engine.resolve(invocation) + rebuilt = engine.rebuild_plan_from_trajectory( + resolved, context, result, replace(result.joint_trajectory, velocities=None) + ) + assert rebuilt.commands.frame_count == result.commands.frame_count + assert torch.equal( + rebuilt.joint_trajectory.velocities[:, join], torch.zeros((2, 2)) + ) + changed = result.joint_trajectory.positions.clone() + changed[0, 0, 0] += 0.001 + with pytest.raises(ValueError, match="reference takeover"): + engine.rebuild_plan_from_trajectory( + resolved, + context, + result, + replace(result.joint_trajectory, positions=changed), + ) + changed_join = result.joint_trajectory.positions.clone() + changed_join[0, join + 2, 0] += 0.001 + with pytest.raises(ValueError, match="reference takeover"): + engine.rebuild_plan_from_trajectory( + resolved, + context, + result, + replace(result.joint_trajectory, positions=changed_join), + ) + zero_context = replace( + context, robot=replace(context.robot, command_qpos=torch.zeros((2, 2))) + ) + already_continuous = engine.plan(invocation, zero_context) + assert ( + already_continuous.diagnostics.metadata["reference_bridge"]["prefix_samples"] + == 0 + ) + assert already_continuous.commands.frame_count == 3 + moving = replace( + context, robot=replace(context.robot, command_qvel=torch.ones((2, 2))) + ) + moving_plan = engine.plan(invocation, moving) + assert moving_plan.commands.frame_count == 3 + assert "reference_bridge_unavailable" in moving_plan.diagnostics.metadata + projected = context.robot.with_qpos(torch.ones((2, 2))) + assert torch.equal(projected.command_qpos, torch.ones((2, 2))) + projected.command_qpos.zero_() + assert torch.equal(context.robot.command_qpos, reference) + + +def test_reference_bridge_retains_physical_gate_after_shifted_phase(monkeypatch): + from embodichain.lab.sim.atomic_skills._expansion_phases import FREE_PHASES + + engine, _ = _engine() + engine.register(PhaseGateAction()) + monkeypatch.setitem(FREE_PHASES, "phase_gate", frozenset({"prepare"})) + engine.robot.get_qvel_limits.return_value = torch.full((1, 2), 0.5) + engine.robot.get_qpos_limits.return_value = torch.tensor( + [[[-3.0, 3.0], [-3.0, 3.0]]] + ) + context = _context(0.0, 0.0, 0.0001, 0, control_dt=0.1) + context = replace( + context, + robot=replace( + context.robot, + command_qpos=torch.full((1, 2), 0.01), + command_qvel=torch.zeros((1, 2)), + ), + ) + invocation = replace( + _phase_gate_invocation(engine), + tracking_policy=replace(TrackingPolicy.timed(), boundary_reference_bridge=True), + ) + invocation = replace( + invocation, + recovery_policy=replace( + invocation.recovery_policy, max_replans=0, max_skill_retries=0 + ), + ) + session = engine.start((invocation,), context) + boundary_index = session.active_plan.segments[1].start + assert boundary_index > 2 + for i in range(boundary_index - 1): + tick = session.tick( + replace(context, robot=replace(context.robot, timestamp=i * 0.1)) + ) + assert tick.pending_phase_effect_gate is None + boundary = session.tick( + replace( + context, robot=replace(context.robot, timestamp=(boundary_index - 1) * 0.1) + ) + ) + request = boundary.pending_phase_effect_gate + assert request is not None and request.segment_name == "commit" + assert request.next_waypoint_index == boundary_index + blocked = session.tick( + replace(context, robot=replace(context.robot, timestamp=boundary_index * 0.1)) + ) + assert blocked.pending_phase_effect_gate is not None + assert torch.equal( + _joint_positions(blocked.command), _joint_positions(boundary.command) + ) diff --git a/tests/sim/atomic_skills/test_tracking.py b/tests/sim/atomic_skills/test_tracking.py index 89e29bf59..55426f92f 100644 --- a/tests/sim/atomic_skills/test_tracking.py +++ b/tests/sim/atomic_skills/test_tracking.py @@ -261,3 +261,40 @@ def test_whole_body_metric_requires_pose_and_joint_acceptance() -> None: assert torch.allclose( evaluation.component_errors["joint_max_abs"], torch.tensor([0.01, 0.2]) ) + + +def test_boundary_policy_snapshot_preserves_independent_terminal_contract(): + from dataclasses import replace + + original = replace( + TrackingPolicy.timed(settle_duration=0.4), + boundary_stop=True, + boundary_position_tolerance=0.002, + boundary_velocity_tolerance=0.01, + boundary_timeout=2.0, + ) + owned = original.snapshot() + assert owned == original and owned is not original + assert owned.terminal is not original.terminal + assert owned.terminal.settle_duration == 0.4 + with pytest.raises(TypeError): + replace(original, boundary_stop=1) + with pytest.raises(ValueError): + replace(original, boundary_timeout=float("nan")) + + +def test_reference_prefix_preserves_endpoints_ownership_and_limits(): + from embodichain.lab.sim.atomic_skills._reference_bridge import _reference_prefix + + start = torch.tensor([[0.01, 0.2]]) + goal = torch.tensor([[0.0, 0.3]]) + prefix = _reference_prefix(start, goal, (0,), 0.04, torch.tensor([0.5, 0.0]), 0.5) + path = torch.cat((prefix, goal[:, None]), dim=1) + assert path[0, 0, 0] == start[0, 0] + assert torch.equal(path[0, :, 1], torch.full_like(path[0, :, 1], 0.3)) + assert torch.equal(path[:, -1], goal) + velocity = torch.diff(path, dim=1) / 0.04 + assert velocity.abs().max() <= 0.5 + assert (torch.diff(velocity, dim=1) / 0.04).abs().max() <= 0.5 + prefix.zero_() + assert start[0, 0] == 0.01 and goal[0, 1] == 0.3 diff --git a/tests/sim/objects/test_scene_backend.py b/tests/sim/objects/test_scene_backend.py index 52697d00b..ab56966cd 100644 --- a/tests/sim/objects/test_scene_backend.py +++ b/tests/sim/objects/test_scene_backend.py @@ -1402,6 +1402,63 @@ class RebuiltScene: assert factory.call_args.args[0] is rebuilt +def test_scoped_link_reads_use_arena_local_rows_and_own_the_result() -> None: + import numpy as np + from scipy.spatial.transform import Rotation + from embodichain.lab.sim.objects.articulation import Articulation + from embodichain.lab.sim.objects.backends._numeric_observations import ( + numeric_observation_scope, + ) + + roots = {} + handles = [] + expected = [] + for row in range(2): + root = np.eye(4, dtype=np.float32) + root[:3, :3] = Rotation.from_euler("z", 0.7 * row).as_matrix() + root[0, 3] = 2.0 * row + local = np.eye(4, dtype=np.float32) + local[:3, :3] = Rotation.from_euler("x", 0.2 + row).as_matrix() + local[1, 3] = 1.0 + row + roots[str(row)] = Mock() + roots[str(row)].get_root_node.return_value.get_world_pose.return_value = root + handles.append( + SimpleNamespace( + arena_name=str(row), get_link_pose=Mock(return_value=root @ local) + ) + ) + expected.append(torch.from_numpy(local)) + scene = SimpleNamespace(get_arena=roots.__getitem__, topology_revision=0) + data = SimpleNamespace( + articulation_view=SimpleNamespace(batch=object(), scene=scene) + ) + robot = SimpleNamespace( + body_data=data, _data=data, _entities=handles, link_names=["base"] + ) + sim = SimpleNamespace( + physics_backend="default", + device=torch.device("cpu"), + physics_step_index=0, + get_rigid_object_uid_list=lambda: [], + ) + with numeric_observation_scope(sim, robot): + rows = [1, 0, 1] + poses = Articulation.get_link_pose(robot, "base", env_ids=rows, to_matrix=True) + torch.testing.assert_close(poses, torch.stack([expected[i] for i in rows])) + poses.zero_() + observed = Articulation.get_link_pose( + robot, "base", env_ids=torch.tensor([1]), to_matrix=True + ) + torch.testing.assert_close(observed[0], expected[1]) + handles[1].get_link_pose.assert_called_once_with("base") + sim.physics_step_index += 1 + handles[0].get_link_pose.return_value = np.eye(4, dtype=np.float32) + observed = Articulation.get_link_pose( + robot, "base", env_ids=[0], to_matrix=True + ) + torch.testing.assert_close(observed[0], torch.eye(4)) + + def test_numeric_boundary_owns_samples_and_invalidates_writes_and_revisions() -> None: from contextvars import copy_context from embodichain.lab.sim.objects.backends._numeric_observations import (