diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 60a5145e0..b236d210c 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -427,6 +427,8 @@ topics: - atomic actions - motion primitive - action primitive + - object semantics + - scene grounding - AtomicAction - ActionInvocation - AtomicActionEngine @@ -451,6 +453,16 @@ topics: - SceneSnapshotSupplier - SceneProvider - RigidObjectSceneProvider + - ObjectSemantics + - entity_id + - frozen ObjectSemantics + - legacy uid + - stable entity identity + - snapshot grounding + - AssembleGoal + - AssembleAffordance + - base_pose + - _scene_dependencies - collision world revision - dynamic obstacle - StateDelta @@ -485,6 +497,8 @@ topics: source_of_truth: - embodichain/lab/sim/atomic_actions/core.py - embodichain/lab/sim/atomic_actions/goals.py + - embodichain/lab/sim/atomic_actions/effects.py + - embodichain/lab/sim/atomic_actions/affordance.py - embodichain/lab/sim/atomic_actions/bindings.py - embodichain/lab/sim/atomic_actions/control.py - embodichain/lab/sim/atomic_actions/invocation.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 112d2596b..4213c7477 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -71,6 +71,76 @@ to the skill-specific `_plan()` hook. New actions implement `_plan()` and must not override `plan()`. `engine.plan_action(...)` is only an extension/testing escape hatch for an unregistered instance. +The `_plan()` extension boundary is an intentional hard break with no legacy +adapter. A subclass that defines `plan()` raises `TypeError` at class definition; +migrate an older custom action by renaming that implementation to `_plan()`. + +## Object identity and pose grounding + +`ObjectSemantics.entity_id` is the canonical pre-registry snapshot key. It is +optional for direct-core compatibility but, when supplied, must be a non-empty +string. Pose grounding with an explicit ID is strict: resolve it only from the +current `PlanningContext.scene`; a missing snapshot entry is an error and never +falls back to the live `entity`. Only when no ID is supplied may the core read +`ObjectSemantics.entity`; that path emits `DeprecationWarning`, reads live state, +and cannot declare a scene-motion dependency. + +`ObjectSemantics` is shallow-frozen. Top-level fields such as `entity_id`, +`entity`, and `label` cannot be rebound after construction; create a new +semantics value to change identity. Nested affordance and metadata objects may +remain mutable, but they never establish identity. + +`SceneSnapshot` owns copies of its input poses, but `EntityState.pose` tensors +are not deeply read-only. Callers must treat published snapshot values as +immutable and publish a newer scene version for changes. Enforced deep +immutability is deferred to the SceneRegistry/snapshot hardening phase. + +Stable object identity follows these exact rules: + +1. The same `ObjectSemantics` instance is identical to itself. +2. If either side has an explicit `entity_id`, both sides must have an explicit + ID and the strings must match. Never compare an explicit ID directly with a + legacy UID, even when the spellings are equal. +3. Only when both explicit IDs are absent, compare non-empty legacy + `entity.uid` values. If either side has a valid UID, both must have one and + the strings must match. +4. Only when neither side has an explicit ID or valid UID may identity fall back + to the same live entity handle. `label` is descriptive and never establishes + identity. + +This is a snapshot/identity bridge, not alias resolution. A future +`SceneRegistry` owns uniqueness, aliases, normalization, and authoritative +registry IDs. Partial-batch `StateDelta` attachment merges use the same stable +identity rules, so equivalent semantic wrappers update one held object instead +of creating label-based duplicates. + +For both individual and coordinated attachments, a same-identity partial merge +preserves scalar metadata: if any previously active environment row remains, +the merged relation keeps `previous.semantics` and selects only the per-row +mask, transforms, and grasp poses from previous/candidate values. It adopts +`candidate.semantics` only when no previously active row survives the update. +This prevents an update for some environments from silently replacing the +semantic metadata shared by untouched rows. + +Scene dependencies must match the poses each primitive actually consumes: + +| Primitive | Scene dependencies | +|---|---| +| `MoveEndEffector` | A `SceneEntityPose` in `xpos`. | +| `MoveJoints` | None; its target is qpos or a named control-profile command. | +| `PickUp` | Always its semantic `entity_id`, when present, because the object pose is grounded once and reused; plus any goal-owned `SceneEntityPose`, such as `grasp_xpos`. | +| `CoordinatedPickment` | Goal-owned target/initial `SceneEntityPose` values; the semantic `entity_id` only when `object_initial_pose` is omitted and semantic grounding supplies that pose. | +| `Place` | A `SceneEntityPose` in ordinary `xpos`; for `AssembleGoal`, `base_pose` when supplied. Omitting `base_pose` uses the deprecated live `AssembleAffordance.base_object_entity` fallback with no dependency. | +| `MoveHeldObject` | A `SceneEntityPose` in `object_target_pose`; current object orientation is derived from observed EEF pose plus verified `object_to_eef`, not a scene-object read. | +| `Press` | A `SceneEntityPose` in `xpos`. | +| `CoordinatedPlacement` | `SceneEntityPose` values in the placing or support object target pose. | +| `HandOver` | No semantic-object scene dependency. It verifies stable attachment identity and derives current pose from held state; its middle/final option poses are tensors, and the reused `GraspGoal.grasp_xpos` field is ignored. | + +`collect_scene_dependencies()` deliberately stops at `ObjectSemantics`. +Therefore, a custom action that consumes a snapshot pose through semantic data +must override `_scene_dependencies()`, union `super()` dependencies, and add the +consumed semantic ID. Do not declare an ID merely because semantics are present. + ## Static compilation Built-ins are already registered by their class-level stable `skill_id`; call: @@ -87,6 +157,11 @@ their last successful qpos. Use invocation `skill_options` for multiple variants with the same stable `skill_id`; do not create per-variant built-in instances. +Composite actions allocate their named trajectory segments from the total +sample budget with `split_three_segments()`. The first motion allocation rounds +`(sample_count - hand_interp_steps) * first_segment_ratio`; callers must not +reproduce that calculation or assume truncation. + ## Dynamic execution and recovery `SceneEntityPose(entity_id, relative_pose)` is resolved from the latest scene @@ -282,7 +357,17 @@ tutorial may derive a simple profile from limits explicitly. `GraspGoal.grasp_xpos` accepts an explicit pose tensor, a late-bound `SceneEntityPose`, or `None` for affordance sampling. A `SceneEntityPose` registers the referenced entity as a recovery dependency, allowing an executing -`PickUp` to replan when the grasp target moves. +`PickUp` to replan when the grasp target moves. `PickUp` also resolves its +semantic object's pose once per planning attempt and declares the semantic +`entity_id` because grasp sampling, upright adjustment, and the held +`object_to_eef` relation all consume that same pose. + +`AssembleGoal.base_pose=SceneEntityPose(...)` is the canonical assembly anchor +and becomes a recovery dependency. An omitted `base_pose` permits the deprecated +live `AssembleAffordance.base_object_entity` fallback for direct-core callers +only; it is not dependency-tracked. The current `assemble.py` tutorial exercises +that legacy fallback, while `moving_target_recovery.py` is the canonical +snapshot-grounded object example. ## Extension rules @@ -292,13 +377,18 @@ registers the referenced entity as a recovery dependency, allowing an executing 4. Implement `_plan()`; do not override the framework-owned `plan()` method. 5. Validate with `require_goal(request)` and consume only the resolved binding. 6. Plan from `context.robot.qpos`; never read an implicit live start state. -7. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. +7. If planning consumes a semantic object's snapshot pose, override + `_scene_dependencies()`, preserve `super()` dependencies, and add exactly + that semantic ID. +8. Return full-robot positions or a `TimedTrajectory` through `build_plan()`. Build batched `list[PlanState]`, translate the policy with `request.motion_policy.to_motion_gen_options()`, and call `self.motion_generator.generate()`. Import pure operations directly from `trajectory_ops.py`. -8. Declare symbolic changes with `StateDelta`; do not mutate context or commit - physical effects during planning. -9. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the +9. Declare symbolic changes with `StateDelta`; do not mutate context or commit + physical effects during planning. For partial attachment updates, retain + previous scalar semantics while any previous row remains; merge only batched + masks/transforms and adopt candidate semantics only on full replacement. +10. Keep scene stepping, controller I/O, and task-graph/MLLM logic outside the atomic action. Put execution-loop I/O behind the runner protocols rather than calling a simulator or device from `plan()` or `ExecutionSession`. diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index 4f5644cc2..6b10e5524 100644 --- a/agent_context/topics/motion-planning/motion-planning.md +++ b/agent_context/topics/motion-planning/motion-planning.md @@ -126,6 +126,9 @@ them into `CuroboPlanOptions.dynamic_obstacle_poses`. to the backend hook. Atomic actions use that facade from their framework-owned `plan()` template when a `SceneSnapshot` declares collision entities; individual skills must not construct backend obstacle options themselves. +`CuroboWorldCfg` rejects duplicate obstacle names and requires every +`dynamic_obstacle_name` to match an object registered in `rigid_objects`, so a +planner-local mismatch fails before backend construction. `MotionGenerator.resolve_plan_options()` is the corresponding option-ownership boundary. It copies caller-supplied typed options, otherwise obtains backend @@ -261,6 +264,9 @@ The decorator checks that every `PlanState` in `target_states` shares the same l accepts only `EEF_MOVE` and `JOINT_MOVE` and raises for other target types. - **Missing interpolation inputs** — `strategy="ik_interp"` requires explicit `start_qpos` and `sample_count`; it never reads live robot state implicitly. +- **CUDA requested on a CPU-only runtime** — planner success-mask normalization + raises a direct `ValueError` before querying the active CUDA device. It never + silently falls back to CPU. - **Constraint tolerance** — `is_satisfied_constraint` allows 10% velocity / 25% acceleration overshoot. Dense waypoint trajectories may appear to violate constraints but pass validation. - **Fork safety with GPU sim** — `ToppraPlannerCfg.mp_context=None` defaults to `spawn` on GPU to avoid fork-after-CUDA-init hazards. Force `fork` only when the sim device is CPU or you have verified it is safe. - **cuRobo shared-world mismatch** — World-frame poses may differ solely because replicated arenas are offset. Compare poses after robot-base rebasing: keep `multi_env=False` if they match, and enable it only when robot-relative layouts differ. diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index 66c78b4e4..21510e7b4 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,10 +1,12 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime - Status: design plan -- Baseline: `main@26b69c22d7efbf96cb35f5487f6922c8645f91d7` +- Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` - Last updated: 2026-08-10 - Related issues: [#471](https://github.com/DexForce/EmbodiChain/issues/471), [#474](https://github.com/DexForce/EmbodiChain/issues/474) +- Related implementation: + [#475](https://github.com/DexForce/EmbodiChain/pull/475) ## 1. Executive summary @@ -88,8 +90,8 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is based on commit `26b69c22` rather than uncommitted working-tree -changes. +This plan is updated against committed `main@e445133c` after PR #475 rather +than uncommitted working-tree changes. | Capability | Current main | Design consequence | |---|---|---| @@ -97,24 +99,32 @@ changes. | Lazy `DemoSegment` execution and legacy demo compatibility (#460) | Available | Use a thin demo adapter; do not create a second dataset executor. | | Closed-loop `ExecutionRunner` and simulator ports (#449) | Available | `SkillRuntime` wraps/reuses the runner rather than scheduling commands itself. | | Dynamic scene recovery and `DynamicCollisionMode` (#450) | Available | Profiles select precise collision semantics and fail early when required capabilities are unavailable. | +| Refined planning architecture (#475) | `MotionGenerator.generate()` is the single planning facade; each `ActionPlan` owns one trajectory and one recovery boundary; named `TrajectorySegment`s are metadata | Do not reintroduce `TrajectoryBuilder`, `MotionPlanningAdapter`, or trajectory-segment recovery. | | Environment cadence through `BaseEnv.step_dt` (#472) | Available | Expert configuration does not expose a separate control period. | | Adaptive dynamic-object settling (#470) | Reset/event implementation exists | Extract a reusable monitor; demo post-policies must advance through `env.step()`. | | Repeated cube pick/place demo | Manually constructs invocations and transform math | First configuration-only vertical slice. | | Open Drawer task (#473) | Manually builds approach, grasp, pull, and command trajectories | Evidence that the semantic layer needs articulation/link/affordance references and a reusable articulation skill. | | Action Bank | Configuration plus task-specific Python node/edge functions | Keep only as a compatibility path while semantic coverage is built. | -Several #474 findings remain prerequisites on this baseline: +PR #475 resolved cumulative translation/rotation publication, removed the dead +`MotionPolicy.interpolation` field, and unified strategy dispatch. It also made +`AtomicAction.plan()` framework-owned and `_plan()` the only custom-action +extension hook. Rejecting a subclass that overrides `plan()` is an intentional +hard break: the project will not provide a compatibility adapter or deprecation +window for that former extension contract. Custom actions must migrate to +`_plan()` so framework-owned scene binding cannot be bypassed. + +The remaining #474 prerequisites on this baseline are: -- `RigidObjectSceneProvider` still updates its pose baseline on every snapshot, - so repeated sub-threshold movement may never publish a revision. -- `AtomicAction` rejects the formerly documented `plan()` extension override - and requires `_plan()` without a compatibility window. - scene pose, semantics, affordance, and collision registration still have multiple sources of truth; - ordinary callers still see a large low-level public surface and must perform semantic transform and verifier plumbing; -- `MotionPolicy` still exposes implementation-level tuning, including an - unused/misleading interpolation option. +- dynamic-obstacle validation is planner-local; provider collision entity IDs + and planner-declared names are not yet fully cross-validated at integration + construction time; +- `MotionPolicy` still exposes implementation-level tuning that should be + hidden behind semantic presets for ordinary users. One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, @@ -130,9 +140,12 @@ The following #471 decisions remain valid: - lazy re-observation when later goals depend on physical effects; - distinct action-effect verification, segment post-policy, and task-level validation responsibilities; -- named phases instead of trajectory indices; +- stable named trajectory segments for tracing instead of recomputed trajectory + indices; - sequential execution first, then resource-aware parallel execution; -- continued legacy compatibility during migration. +- continued Action Bank compatibility and only the explicitly documented + direct-core fallbacks during migration. This does not include the intentional + `plan()` to `_plan()` hard break. The following parts must be adjusted: @@ -145,6 +158,22 @@ The following parts must be adjusted: | Callers may supply place EEF poses and pickup look-ahead options. | `Place` is object-centric; the compiler derives EEF targets from verified held state and propagates downstream targets automatically. | | Configuration and handwritten code are separate entry paths. | Both construct the same semantic call specification and converge before binding or grounding. | +### 5.1 Segment terminology after #475 + +The design uses three different segment layers. Bare "segment" should be +avoided wherever the layer would be ambiguous. + +| Term | Type | Meaning | +|---|---|---| +| Program segment | `SegmentCfg` | Expert Program logical transaction boundary; owns post-policies, validators, and re-observation semantics. | +| Demo segment | `DemoSegment` | Lazy Gym/demo executor carrier and dataset boundary produced from a program segment. | +| Trajectory segment | `TrajectorySegment` | Named half-open waypoint range within one `ActionPlan`; used for inspection, visualization, tracing, and terminal-effect correlation only. | + +A trajectory segment is not an independent planning, recovery, effect, or +timeout boundary. One atomic action remains the recovery/effect boundary. +"Phase 0" through "Phase 8" below refer only to implementation-plan stages; +atomic motion structure is called a trajectory segment, not a phase. + ## 6. Proposed architecture ### 6.1 API layers @@ -250,18 +279,62 @@ SceneEntityRef Rules: -1. An entity is registered once. Planner obstacles, scene dependencies, effect - monitors, and semantic calls consume that registration. -2. Grounding reads pose and geometry from one immutable snapshot. It must not +1. The registry ID is the authoritative entity identity used by semantic calls, + snapshots, scene dependencies, effect monitors, and planner obstacles. An + entity is registered once under that ID. +2. A simulation object's existing `uid` may be imported as a legacy alias only. + Aliases are resolved once at an integration boundary and normalized to the + registry ID; they never replace the authoritative ID. Duplicate registry IDs, + ambiguous aliases, or an alias colliding with another registry ID fail during + registry construction. +3. Grounding reads pose and geometry from one immutable snapshot. It must not mix a snapshot with a live simulation entity pose. -3. Automatic grasp selection declares a target dependency automatically. -4. Dynamic collision setup is derived and cross-validated at construction - time. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the - registry declares dynamic collision entities and fails early if the active - planner cannot satisfy it. -5. Environment scene configuration should populate the registry automatically; +4. Automatic grasp selection declares a target dependency automatically. +5. Dynamic collision setup is derived from authoritative registry IDs. Registry + construction performs the complete provider/planner cross-validation: the + registry's dynamic-collision ID set, the provider's `collision_entity_ids`, + and the planner's dynamic-obstacle names must agree after alias normalization; + every ID must have the required geometry, and the selected planner must + support the declared update mode. The current + planner-local name check remains a lower-level defensive validation, not the + integration contract. +6. The `safe` preset requests `DynamicCollisionMode.REQUIRED` when the registry + declares dynamic collision entities and fails early if the active planner + cannot satisfy it. +7. Environment scene configuration should populate the registry automatically; explicit providers are reserved for perception and hardware integration. +Before PR2A introduces this registry, PR1 provides only a core migration bridge. +`ObjectSemantics.entity_id` is a caller-supplied `SceneSnapshot` key, not yet a +registry reference. `ObjectSemantics` is shallow-frozen so top-level fields, +including `entity_id`, cannot be rebound after attachment state captures the +semantics; identity changes require a new instance. Nested affordance and +metadata objects remain mutable but never establish identity. + +For object identity, explicit and legacy namespaces stay separate. If either +side supplies `entity_id`, both sides must supply the same explicit ID; a +same-spelled simulation `entity.uid` is not sufficient. Only when both explicit +IDs are absent may the bridge compare non-empty legacy UIDs, requiring both UIDs +to exist and match. Only when neither side has an explicit ID or valid UID may +comparison fall back to the same semantic object or live entity handle. +Semantic labels are never identity. Arbitrary alias mapping, uniqueness +enforcement, and normalization to an authoritative registry ID belong to PR2A. + +For pose grounding, an explicit `entity_id` is strict: the pose comes only from +the current versioned `PlanningContext.scene`, and a missing entry is an error. +The planner never falls back to a live entity after an explicit ID fails. A live +`ObjectSemantics.entity` read remains temporarily available, with a deprecation +warning and without a scene dependency, only when no `entity_id` was supplied. +The same boundary applies to `AssembleGoal.base_pose`: the snapshot reference is +canonical, while an omitted reference permits the deprecated direct-core +`AssembleAffordance.base_object_entity` path. + +The current `SceneSnapshot` owns copies of input pose tensors, but exposed +`EntityState.pose` tensors are not deeply read-only. PR1 therefore requires +callers to treat snapshot values as immutable and uses the scene version for +publication/recovery semantics. Enforced deep immutability belongs to the PR2A +registry/snapshot hardening rather than this bridge. + ### 7.2 Robot skill profiles A `RobotSkillProfile` is reusable per embodiment and contains: @@ -292,6 +365,14 @@ EEF pose from the requested object-space target and the verified `object_to_eef` relation. Task code and configuration never perform `desired_object_pose @ object_to_eef`. +As the core migration path for assembly, `AssembleGoal` gains +`base_pose: SceneEntityPose | None`. The semantic compiler always supplies a +`SceneEntityPose` containing the authoritative base-object registry ID, so the +base pose is resolved from the same immutable snapshot and automatically becomes +a scene dependency. `None` preserves the existing live +`AssembleAffordance.base_object_entity` lookup only for legacy direct-core +callers; the semantic facade and Expert Program never emit that fallback. + The workflow compiler inspects later calls and propagates downstream object targets to pickup/grasp selection. The caller does not repeat later goals in `PickUpOptions`. @@ -304,7 +385,8 @@ Compilation has two stages. - validate references, presets, capabilities, resources, and bounded loops; - infer ordering and data/effect dependencies; - propagate downstream object goals for grasp selection; - - identify static stages versus observation-dependent boundaries; + - identify every call boundary that requires fresh observation or verified + effects, without coalescing calls in Version 1; - reject ambiguous bindings and unsupported semantic relations before execution. 2. **Runtime grounding and lowering** @@ -314,11 +396,18 @@ Compilation has two stages. - lower to a typed `ActionInvocation`; - dispatch through the canonical `SkillRuntime`. -Static `engine.compile()` is valid only when later goals do not depend on -observations or effects produced by earlier calls. `engine.start()` and observed -execution are required for grasp/release verification, moving targets, -recovery, post-settling, or any JIT-grounded goal. The default mode is `auto`: -the compiler partitions safe static stages and inserts observed boundaries. +Version 1 executes exactly one semantic call per `ExecutionSession`. The runtime +captures a fresh registry snapshot, lowers one call to one `ActionInvocation`, +constructs a one-invocation session, drives it through terminal effect +verification, commits the verified per-environment task state, and only then +advances to the next call. It never places multiple semantic calls in one +`ExecutionSession`. + +Static `engine.compile()` remains an advanced core API for explicitly +observation-independent offline planning. The Version 1 semantic runtime does +not coalesce calls into static stages; such an optimization requires a later +design proving that it preserves the call, effect, and re-observation +boundaries. ### 7.5 Skill runtime @@ -328,10 +417,11 @@ the compiler partitions safe static stages and inserts observed boundaries. - synchronous `run(...)` and non-blocking `step()` entry points; - planning-context refresh through registered observation ports; - JIT lowering of the next semantic call; +- exactly one semantic call and one invocation per `ExecutionSession`; - persistent, per-environment verified `TaskState`; - built-in effect-monitor selection and feedback to `ExecutionSession`; - uniform `SkillResult`, cancellation, timeout, and safe-stop behavior; -- semantic and named-phase events. +- semantic action events and optional trajectory-segment trace metadata. Catalog discovery and runtime installation should have distinct names. For example, a catalog can `discover` a descriptor while an engine explicitly @@ -462,13 +552,14 @@ examples. Stable names should be preferred over internal fields: ```yaml advanced: - phase_presets: - secure_grasp: precise + call_presets: + pick: precise recovery_preset: dynamic_scene ``` Raw planner instances, callables, arbitrary imports, and environment paths are -never serializable configuration values. +never serializable configuration values. Version 1 does not attach motion or +recovery policies to individual `TrajectorySegment`s. ## 9. Demonstration execution semantics @@ -482,8 +573,8 @@ Gym-aware runtime ports: - command sink: buffers the next full-robot command for the environment action manager; - clock: advances only when the demo executor calls `env.step()`; -- metadata sink: records compiler decisions, phases, effects, recovery, scene - revisions, and post-policy results. +- metadata sink: records compiler decisions, action trajectory segments, + effects, recovery, scene revisions, and post-policy results. The existing `SimulationExecutionAdapter` is not the demo execution loop because direct simulator updates can bypass environment managers and recorders. @@ -501,23 +592,39 @@ per yielded command. An incompatible command is rejected with a clear timing error; it is not silently resampled. Explicit timed-command resampling can be a later, separately tested feature. -Timeout for a named phase starts when its first command is dispatched, not when -an earlier phase or the whole segment is compiled. +Recovery timeout and retry budgets are scoped to the enclosing action attempt. +A `TrajectorySegment` does not start an independent timer or own a recovery +policy. Program-segment settling and validation use separate post-policy +deadlines. -### 9.3 Named phases +### 9.3 Named atomic trajectory segments -Plans and execution events need stable semantic phase names. Initial built-ins -should expose at least: +Version 1 freezes the trajectory-segment names already emitted by current +built-ins. A successful non-empty plan exposes the following ordered names; +zero-length optional segments are omitted: -- pick: `approach`, `grasp_close`, `lift`; -- place: `lower`, `release`, `retract`; -- handover: role-specific approach, transfer, release, and retreat phases; -- articulation operation: `approach`, `grasp_close`, `operate`, `release`, - `retract`. - -Post-policies and effect monitors subscribe to names, not trajectory sample -indices. The runtime validates requested phase names against the active skill -descriptor before execution. +| Atomic skill ID | Ordered trajectory-segment names | +|---|---| +| `move_joints` | `move_joints` | +| `move_end_effector` | `move_end_effector` | +| `move_held_object` | `transport` | +| `pick_up` | `approach`, `close`, `lift` | +| `place` (including `AssembleGoal`) | `approach`, `release`, `retract` | +| `press` | `close`, `press`, `retract` | +| `hand_over` | `transfer`, `approach`, `close`, optional `hold`, `release`, `deliver` | +| `coordinated_pickment` | `approach`, `close`, `lift`, `move`, optional `hold` | +| `coordinated_placement` | `approach`, optional `hold`, optional `release`, `retreat` | + +These spellings are a trace/metadata contract. Renaming or removing one requires +an explicit API review and migration rather than a silent change in a primitive. + +Names are validated by `ActionPlan`; ranges may change after replanning when a +backend returns a different sample count. Effect monitors run at the action +effect boundary and may use `EffectVerificationRequest.terminal_segment` for +correlation. Program post-policies and validators subscribe to program/demo +segment boundaries, not trajectory segments. Articulation segment names should +be stabilized with the reusable articulation skill rather than predeclared in +the configuration schema. ### 9.4 Dynamic settling @@ -541,15 +648,22 @@ clear object dynamics. All runtime state is indexed by stable environment IDs: - scene revisions and active collision dependencies; -- current call/phase and command deadline; +- current program segment, semantic call, action waypoint, and command deadline; - recovery budgets and failure masks; - verified held-object/effect state; - post-policy progress and segment validation; - result and metadata. -One environment may finish, recover, settle, or fail without blocking or -overwriting another. Program structure is shared, but runtime progress is -masked per environment. +Version 1 uses a shared program/call barrier for the environment batch; it does +not maintain a divergent AST program counter or a separate `ExecutionSession` +per environment. The runtime advances to the next semantic call or program +segment only when every still-eligible active row reaches the current boundary. +A slower or recovering active row therefore keeps the batch at that boundary. + +Within the shared barrier, task state, effects, recovery budgets, eligibility, +success, and failure remain independent per environment. Completed, failed, or +otherwise inactive rows emit hold behavior and cannot overwrite another row's +state while the active cohort catches up. ## 10. Action Bank migration @@ -558,7 +672,7 @@ capability parity. | Action Bank concept | Expert Program / semantic runtime | |---|---| -| scope | `Segment` or nested `Sequence` | +| scope | Program `SegmentCfg` or nested `SequenceCfg` | | custom node function | registered semantic call and shared compiler | | custom edge/target function | typed target provider or goal grounder | | graph edge | explicit sequence/effect dependency inferred by compiler | @@ -573,7 +687,10 @@ Migration rules: working during the transition. 2. Add `EmbodiedEnvCfg.expert_program` and a CLI input such as `--expert_program`; reject simultaneous legacy and new program inputs. -3. Migrate sequential tasks first and compare generated metadata and outcomes. +3. Do not require official-task migration in PR1. Start opt-in sequential-task + migration with the repeated-cube vertical slice after the registry, compiler, + runtime, and demo bridge contracts are available, then compare generated + metadata and outcomes. 4. Add `Parallel` only with deterministic resource conflict checks, trajectory alignment, synchronization barriers, and per-environment `StateDelta` merging. @@ -615,12 +732,16 @@ Each item below should remain a focused PR with its own public-API review and tests. The dependency order is: ```text -Phase 0 correctness +Phase 0 correctness (complete) | v -SceneRegistry + RobotSkillProfile +PR1 snapshot/identity bridge | - v + +-----------------------+ + v v +PR2A SceneRegistry PR2B RobotSkillProfile + +-----------+-----------+ + v Semantic calls/compiler --> SkillRuntime/effect monitors | | +---------------+--------------+ @@ -638,36 +759,117 @@ Semantic calls/compiler --> SkillRuntime/effect monitors Action Bank deprecation ``` -### Phase 0: correctness and compatibility prerequisites - -Deliverables: +### Phase 0: correctness and core-contract decisions (complete) + +Landed on `main` through #475: + +- cumulative sub-threshold translation and rotation compare against the last + published pose; +- target/general-scene and per-environment collision revisions have regression + coverage; +- the dead `MotionPolicy.interpolation` field is removed and strategy dispatch + is unified; +- one action owns one trajectory and one recovery/effect boundary, while named + `TrajectorySegment`s remain metadata; +- `_plan()` is the only supported custom-action extension hook. The immediate + class-definition failure for a legacy `plan()` override is a documented, + tested, intentional hard break with no compatibility adapter; +- planner-local dynamic-obstacle name validation remains in place as a defensive + core check. Complete provider/planner cross-validation is deliberately owned + by the authoritative `SceneRegistry` integration in Phase 1. + +Exit criteria are met on `main@e445133c`. Implementation may proceed to the +focused PR1 bridge without adding a legacy `plan()` adapter or a pre-registry +duplicate of the integration-level obstacle validator. + +### PR1: core snapshot and identity bridge + +PR1 is deliberately smaller than Phase 1. It establishes the core seams that +the later registry and profile integrations consume: + +- add optional, validated `ObjectSemantics.entity_id` as the stable + `SceneSnapshot` key for canonical object grounding; +- resolve explicit IDs only from `PlanningContext.scene`, with a hard error and + no live fallback when the snapshot entry is missing; +- keep `ObjectSemantics.entity` only as a deprecated no-ID compatibility path; +- shallow-freeze `ObjectSemantics` fields so captured `entity_id` values cannot + be rebound without constructing a new semantic value; +- define stable held-object identity and partial-batch `StateDelta` merging: + if either side has an explicit `entity_id`, both explicit IDs must exist and + match; only two explicit-ID-less values may compare matching legacy + `entity.uid` strings, and only values with neither ID form may fall back to the + same semantic object or live handle; +- preserve scalar semantics during same-identity partial `StateDelta` merges: + while any previously active row remains, retain `previous.semantics` and + merge only per-environment masks, transforms, and grasp poses; adopt + `candidate.semantics` only when all previously active rows are replaced; +- add an action-owned scene-dependency hook. `PickUp` declares its semantic + object ID, coordinated pickup declares it only for the implicit initial-pose + path, and goal-owned `SceneEntityPose` values remain automatic dependencies; +- resolve each pickup object pose once per planning attempt and reuse that + tensor for grasp sampling, upright adjustment, and `object_to_eef`; +- derive held-object pose for `MoveHeldObject` and `HandOver` from the observed + EEF pose and verified `object_to_eef` instead of a live entity read; +- add `AssembleGoal.base_pose: SceneEntityPose | None`; the explicit reference + is snapshot-backed and dependency-tracked, while `None` retains the deprecated + `AssembleAffordance.base_object_entity` fallback; +- add focused tests, documentation, and one canonical snapshot-grounded moving + target tutorial. Keep `scripts/tutorials/atomic_action/assemble.py` explicitly + documented as a legacy fallback example until its later registry migration. + +PR1 does not add a `SceneRegistry`, a `SceneEntityRef` hierarchy, alias maps, +cross-source uniqueness or collision validation, a `RobotSkillProfile`, or +semantic presets. It does not require official task environments to migrate; +they remain on the compatibility path until a later opt-in vertical slice. + +Exit criteria: canonical object grounding never mixes snapshot and live poses; +explicit missing IDs fail; dependency metadata matches the poses actually +consumed; stable-identity merges are deterministic; and existing direct-core +callers remain usable only through the documented deprecated fallbacks. -- fix cumulative sub-threshold translation and rotation publication in - `RigidObjectSceneProvider` by comparing with the last published/significant - pose; -- add regression tests for target and collision-world revisions; -- decide the supported `plan()`/`_plan()` custom-action extension contract and - provide a compatibility/deprecation path before enforcing a break; -- remove or implement misleading `MotionPolicy` fields, keeping collision - semantics expressed by `DynamicCollisionMode`; -- add early cross-validation for registry/provider/planner obstacle names. +### Phase 1: unified integration data -Exit criteria: all #474 P0 items are resolved on main and custom actions have a -documented, tested upgrade path. +Phase 1 is implemented as two focused follow-up PRs that join before the +semantic facade/compiler work. -### Phase 1: unified integration data +#### PR2A: SceneRegistry Deliverables: - `SceneEntityRef` hierarchy and `SceneRegistry`; -- immutable snapshot as the only grounding pose authority; -- environment-to-registry population and collision/provider derivation; -- `RobotSkillProfile`, capability-based binding, semantic tool commands, and - stable presets; +- authoritative registry IDs with simulation `uid` values accepted only as + normalized legacy aliases; +- immutable snapshots as the only grounding pose authority for the canonical + semantic/compiler path; +- opt-in environment-to-registry population and collision/provider derivation; +- complete construction-time agreement checks across registry collision IDs, + provider collision IDs, planner dynamic-obstacle names, geometry, and planner + capability; - explicit catalog-discovery versus engine-installation terminology. -Exit criteria: an object is registered once and a dynamic-object configuration -error fails before execution with an entity-centric diagnostic. +`ObjectSemantics.entity_id` and `AssembleGoal.base_pose` already provide the +lowering targets from PR1. PR2A replaces manually coordinated IDs/providers with +one authoritative registration and performs alias normalization exactly once at +the integration boundary. + +#### PR2B: RobotSkillProfile + +Deliverables: + +- `RobotSkillProfile` and reusable capability declarations; +- capability-based deterministic binding and explicit ambiguity errors; +- semantic tool commands and stable runtime/planning presets; +- profile validation against installed engine skills and robot control parts. + +PR2B may proceed in parallel with PR2A after the PR1 bridge. Neither follow-up +requires official task migration; the repeated-cube vertical slice opts in only +after the registry, profile, compiler, runtime, and demo bridge are available. + +Combined Phase 1 exit criteria: an object is registered once under an +authoritative ID, aliases cannot introduce ambiguity, dynamic-object +configuration mismatches fail before execution with an entity-centric +diagnostic, and robot capabilities resolve bindings/presets without task-owned +motion code. ### Phase 2: semantic facade and compiler @@ -690,9 +892,12 @@ effect verifier. Deliverables: - `SkillRuntime` wrapping `ExecutionRunner` for sync and step-wise use; +- exactly one semantic call lowered to one invocation in one + `ExecutionSession`; - built-in simulation effect monitors for grasp, release, and handover; - uniform per-environment `SkillResult` and persistent verified `TaskState`; -- automatic static/observed stage selection; +- a shared Version 1 program/call barrier with independent per-environment task, + effect, recovery, eligibility, and result state; - safe cancellation, timeout, and hold behavior inherited from the runner. Exit criteria: Python calls and a programmatic `SemanticCallSpec` use identical @@ -702,16 +907,17 @@ compiler/runtime code and produce equivalent results. Deliverables: -- stable named phases in plans/descriptors/events; +- expose the existing named plan trajectory segments through optional demo + trace metadata without adding segment-level recovery; - reusable `DynamicSettleMonitor` shared by reset and demo paths; - Gym observation, buffered command, and environment-clock ports; - thin `AtomicDemoBridge` yielding lazy `DemoSegment`s; - exact `BaseEnv.step_dt` timing validation; -- runtime metadata for calls, phases, effects, recovery, scene revisions, - settling, and validation. +- runtime metadata for calls, trajectory segments, effects, recovery, scene + revisions, settling, and validation. -Exit criteria: no demo command bypasses `env.step()`, and phase/post-policy -behavior contains no hard-coded trajectory index. +Exit criteria: no demo command bypasses `env.step()`, and no post-policy, +effect, or trace integration contains a hard-coded trajectory index. ### Phase 5: Expert Program version 1 and repeated-cube vertical slice @@ -725,12 +931,13 @@ Deliverables: Exit criteria: -- three lazy segments complete in supported simulation; -- each segment re-observes the cube after free-fall settling; +- three lazy program/demo segments complete in supported simulation; +- each program/demo segment re-observes the cube after free-fall settling; - grasp and release effects are verified; - placement uses verified held-object state; - settle and validation data are present in metadata; -- multi-environment success, failure, and recovery masks remain independent; +- the environment batch advances through the shared call barrier while success, + failure, effect, recovery, and eligibility masks remain independent; - the task contains no task-specific motion-generation code. ### Phase 6: sequential skill coverage and articulated interaction @@ -739,7 +946,7 @@ Deliverables: - articulation/link/affordance registry integration; - reusable articulation-operation semantic call, compiler, effect monitor, and - named phases; + named trajectory segments; - configuration-based Open Drawer migration; - migrate additional sequential tasks to reveal missing reusable grounders, monitors, and validators. @@ -781,27 +988,38 @@ independent of adoption of the new path. - strict decoder, unknown fields, schema versioning, bounded repeats, and registry reference errors; +- authoritative registry-ID normalization, legacy-`uid` alias collisions, and + complete registry/provider/planner obstacle-set agreement; - cumulative scene movement and collision dependency revision behavior; - profile capability matching, deterministic binding, and ambiguity errors; -- static versus observed stage partitioning; +- `AssembleGoal.base_pose` snapshot resolution and its automatic scene + dependency, with the `None` fallback isolated to legacy direct-core use; +- same-identity partial `StateDelta` merges retain previous scalar semantics + until every previously active row is replaced, for both individual and + coordinated attachments; +- exactly one semantic call and one invocation per `ExecutionSession`; - downstream target propagation for grasp selection; - object-centric place conversion from one immutable snapshot and verified held state; - effect monitor state transitions and timeout/recovery feedback; -- named phase validation and exact step-duration conversion; +- trajectory-segment coverage/name validation and exact step-duration + conversion; - Action Bank compatibility adapters where introduced. ### Integration tests with fake ports - Python facade and Expert Program lower to equivalent invocations; - runner scheduling, acknowledgement, safe stop, and cancellation are reused; -- one environment can complete while another recovers or fails; +- the Version 1 shared call barrier holds active rows together while completed, + recovering, and failed rows retain independent masks and state; - command buffering advances only through the environment clock; -- segment metadata is deterministic and serializable. +- program/demo-segment and trajectory-segment metadata are deterministic and + serializable. ### Simulation tests -- three-segment repeated cube pick/place with free-fall re-observation; +- three-program/demo-segment repeated cube pick/place with free-fall + re-observation; - moving target and dynamic collision recovery with the `safe` preset; - grasp/release/handover effect monitors; - settling success and timeout metadata; @@ -819,8 +1037,9 @@ The design is complete when all of the following hold: typed atomic-action core, and runtime. - [ ] A common new task using existing semantic skills needs no task-specific motion-generation code. -- [ ] Each scene entity is registered once across semantics, observation, - affordance, and collision handling. +- [ ] Each scene entity is registered once under an authoritative registry ID + across semantics, observation, affordance, and collision handling; + simulation `uid` values are legacy aliases only. - [ ] The default pick/place path does not expose raw qpos, grasp/EEF matrix math, planner construction, session plumbing, or custom verification. - [ ] Automatic grasping tracks target revisions and receives downstream object @@ -828,15 +1047,21 @@ The design is complete when all of the following hold: - [ ] `Place` is object-centric and consumes verified held-object state. - [ ] Built-in grasp, release, handover, and supported articulation effect monitors work in simulation. -- [ ] Repeated sub-threshold motion eventually publishes the correct scene +- [x] Repeated sub-threshold motion eventually publishes the correct scene revision. -- [ ] Custom actions have a documented and tested compatibility path. +- [x] Custom actions have a documented and tested intentional hard-break + migration from overriding `plan()` to implementing `_plan()`; no + compatibility adapter is required. +- [ ] Version 1 creates exactly one one-invocation `ExecutionSession` for each + semantic call and re-observes before lowering the next call. - [ ] Demonstration timing is derived from `BaseEnv.step_dt` and commands pass through `env.step()`. -- [ ] Phase hooks use stable names rather than trajectory indices. +- [ ] No program post-policy, effect, or tracing integration depends on + hard-coded waypoint indices. - [ ] Repeated cube pick/place completes at least three lazy, independently - observed segments with settle/effect/validation metadata. -- [ ] Multi-environment progress, effects, recovery, and failures remain + observed program/demo segments with settle/effect/validation metadata. +- [ ] Version 1 uses one shared program/call barrier while per-environment task + state, effects, recovery, eligibility, success, and failure remain independent. - [ ] Advanced users retain typed goals, invocations, policies, providers, sessions, and planners as escape hatches. @@ -854,7 +1079,7 @@ The design is complete when all of the following hold: | Automatic binding makes surprising choices | Use capability validation and deterministic profile preferences; surface semantic ambiguity rather than silently selecting. | | Presets become opaque or unstable | Version preset semantics, emit the resolved core policies in runtime metadata, and keep typed overrides available to advanced users. | | Built-in effect monitors overfit simulation | Keep the contract backend-neutral and provide replaceable hardware implementations; record monitor evidence and thresholds. | -| Static compilation uses stale state | Default to dependency-driven `auto` partitioning and force observed boundaries after external effects or dynamic post-policies. | +| Static compilation uses stale state | Version 1 never coalesces semantic calls into one session or static stage; keep `engine.compile()` as an explicit advanced-core API until a later optimization proves equivalent observation/effect boundaries. | | Demo bridge duplicates runner logic | Keep scheduling, acknowledgement, recovery, timeout, and safe stop in `ExecutionRunner`; bridge only the Gym step boundary. | | Configuration grows into a programming language | Keep version 1 bounded and discriminated; add only registered nodes and no expressions or arbitrary DAG scheduler. | | Articulation and parallel work delay useful delivery | Ship the sequential cube vertical slice first; add reusable capabilities independently. | diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md index 7e7aeef73..0ad26c50a 100644 --- a/docs/source/overview/sim/atomic_actions/builtin_actions.md +++ b/docs/source/overview/sim/atomic_actions/builtin_actions.md @@ -196,14 +196,45 @@ entity as a recovery dependency. | Skill / field | `SceneEntityPose` accepted | Automatic scene-motion replan | |---|---:|---:| | `MoveEndEffector.xpos` | yes | yes | +| `MoveJoints.target` | no | no | | `MoveHeldObject.object_target_pose` | yes | yes | | `Place.xpos` | yes | yes | | `Press.xpos` | yes | yes | | `CoordinatedPickGoal.object_target_pose` / `object_initial_pose` | yes | yes | | `CoordinatedPlacementGoal` placing/support poses | yes | yes | | `PickUp.grasp_xpos` | yes | yes | -| `PickUp` / `HandOver` `ObjectSemantics.entity` lookup | not through `SceneEntityPose` | no automatic scene dependency | -| `AssembleGoal` base entity lookup | not through `SceneEntityPose` | latest pose is used when replanning, but base movement alone does not trigger it | +| `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; always consumed for the object pose | +| Coordinated pickup implicit initial pose via `ObjectSemantics.entity_id` | implicit snapshot reference | yes; only when `object_initial_pose` is omitted | +| `AssembleGoal.base_pose` | yes | yes | +| Deprecated `ObjectSemantics.entity` / `AssembleAffordance.base_object_entity` fallback | no | no | +| `HandOver` current held-object pose | no scene lookup | no; derived from observed EEF pose and verified attachment state | + +### Object identity and grounding + +`ObjectSemantics.entity_id` is the canonical scene-snapshot key. It must be a +non-empty string when set. An explicit ID is strict: object grounding reads only +`PlanningContext.scene.entities[entity_id]`, and a missing entry is an error. It +never falls back to `ObjectSemantics.entity` after an explicit lookup fails. + +The live `entity` field remains a deprecated direct-core compatibility path only +when `entity_id` is absent. That read emits `DeprecationWarning` and cannot +create a scene-motion dependency. `collect_scene_dependencies()` intentionally +does not recurse into `ObjectSemantics`; each primitive declares a semantic ID +only when its planner actually consumes that object's snapshot pose. + +Attachment and handover identity are not based on `label`. The core resolves an +explicit `entity_id` only against another explicit ID. If either compared side +has one, both sides must have the same explicit value; an equal legacy +`entity.uid` does not match it. When both explicit IDs are absent, two non-empty +legacy UIDs may match. Only when neither side has either ID form may comparison +fall back to the same semantic object or live entity handle. Future +`SceneRegistry` integration will own arbitrary alias normalization; this core +bridge does not. + +`ObjectSemantics` is shallow-frozen. Its top-level fields, including +`entity_id`, cannot be rebound after construction; create a new semantics value +to change identity. Nested affordance and metadata objects remain mutable but +do not participate in identity. ### Parameter ownership @@ -307,7 +338,7 @@ bound manipulator. | Skill ID | `pick_up` | | Goal | `GraspGoal(semantics=..., grasp_xpos=None)` | | Binding | manipulator + end effector role `primary` | -| Precondition | `ObjectSemantics.entity` is set; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | +| Precondition | `ObjectSemantics.entity_id` resolves in the planning snapshot; the deprecated live `entity` fallback remains temporarily; an `AntipodalAffordance` is required when no explicit grasp pose is supplied | | Effect | write `HeldObjectState` for the bound manipulator and clear overlapping coordinated attachment state | | Verification | the attachment effect must be verified during closed-loop execution | @@ -318,6 +349,12 @@ dependency, so material target motion invalidates and replans an executing reachability, and stores the selected `object_to_eef` transform in the expected held-object state. Later object-centric skills reuse that transform. +Set `ObjectSemantics.entity_id` to the same stable ID used by the scene +snapshot. `PickUp` resolves that object pose once per planning attempt, uses the +same tensor for grasp sampling, upright adjustment, and `object_to_eef`, and +automatically records the ID as a scene dependency. An explicit ID never falls +back to a live simulation entity when the snapshot entry is missing. + `PickUp` requires `open` and `grasp` commands on the bound end-effector profile. Important `PickUpOptions` fields: @@ -330,11 +367,13 @@ Important `PickUpOptions` fields: | `downstream_object_target_poses` | Optional future reachability constraints used in grasp selection | | `obj_upright_direction`, `rotate_upright` | Optional orientation-selection behavior | -Reading `ObjectSemantics.entity` remains a live planning lookup rather than an -automatic dependency. Use an explicit `SceneEntityPose` in `grasp_xpos` when -object motion should trigger dynamic-goal replanning. +`ObjectSemantics.entity` without an ID is a deprecated compatibility path. Its +live pose does not create an automatic scene dependency. -**Example:** `scripts/tutorials/atomic_action/pickup.py` +**Example:** `scripts/tutorials/atomic_action/pickup.py` currently exercises the +deprecated entity-only fallback. For canonical snapshot grounding and moving +target recovery, see +`scripts/tutorials/atomic_action/moving_target_recovery.py`. (builtin-move-held-object)= @@ -343,6 +382,9 @@ object motion should trigger dynamic-goal replanning. Moves an already attached object to an object-frame target while keeping the hand closed. The caller specifies the desired **object pose**, not an EEF pose; the action derives `target_object_pose @ object_to_eef` from verified task state. +When upright transport needs the current object orientation, it derives it from +the observed EEF pose and verified `object_to_eef` relation rather than reading +a live scene entity. | Contract | Value | |---|---| @@ -400,26 +442,27 @@ The bound end-effector profile must provide `open` and `grasp`. Important ### Assembly through `Place` -`Place` also accepts `AssembleGoal(affordance=...)`. There is no separate -assembly skill: it derives the assemble-object target from the base object's -live pose and reuses the normal place/release segments. +`Place` also accepts +`AssembleGoal(affordance=..., base_pose=SceneEntityPose("base"))`. There is no +separate assembly skill: it derives the assemble-object target from the base +object's snapshot pose and reuses the normal place/release segments. ```text base_object_pose @ assemble_to_base_pose = assemble_object_target_pose assemble_object_target_pose @ held.object_to_eef = release_eef_pose ``` -The `AssembleAffordance` identifies the base and assemble objects, stores the -relative pose, and must provide `base_object_entity`. A prior verified `PickUp` -must have populated the held object's `object_to_eef` transform. Planning then -declares the same detach effect as a normal place. - -The base entity's current pose is read each time `plan()` runs. Because the -goal does not yet encode that entity through `SceneEntityPose`, base movement by -itself does not invalidate an executing plan; another recovery trigger is -required before the newer pose is resolved. +The `AssembleAffordance` stores the relative assembly pose. A prior verified +`PickUp` must have populated the held object's `object_to_eef` transform. +`base_pose` is resolved from each planning snapshot and automatically becomes a +recovery dependency. Omitting it temporarily falls back to the affordance's +`base_object_entity` with a deprecation warning; that fallback is not a scene +dependency. -**Example:** `scripts/tutorials/atomic_action/assemble.py` +**Example:** `scripts/tutorials/atomic_action/assemble.py` currently exercises +the legacy `base_object_entity` fallback and is not the canonical `base_pose` +form. It remains a compatibility example until the registry-backed tutorial +migration. (builtin-press)= @@ -458,7 +501,7 @@ both hands -> lift -> move object -> hold**. | Skill ID | `coordinated_pickment` | | Goal | `CoordinatedPickGoal` | | Binding | manipulator + end effector roles `left` and `right` | -| Precondition | `ObjectSemantics.entity` is set and the affordance is an `AntipodalAffordance` | +| Precondition | an `AntipodalAffordance`; when `object_initial_pose` is omitted, `ObjectSemantics.entity_id` resolves in the snapshot or the deprecated no-ID live fallback is available | | Goal geometry | shared-object target pose and optional initial object pose; left/right grasps are sampled from the affordance | | Effect | clear individual left/right attachments and create `CoordinatedHeldObjectState[(left, right)]` | | Verification | coordinated attachment must be externally verified | @@ -471,9 +514,13 @@ lowest-cost grasp on each side. The derived `object_to_eef` transforms are stored in the projected `CoordinatedHeldObjectState` and reused by later object-centric skills. -The object target and optional initial pose may use `SceneEntityPose`. When no -initial pose is supplied, `ObjectSemantics.entity` provides the object's current -pose. +The object target and optional initial pose may use `SceneEntityPose`. Those +references declare their own scene dependencies. When `object_initial_pose` is +omitted, the action grounds the initial pose from +`ObjectSemantics.entity_id` and declares that ID as a dependency; the deprecated +no-ID `entity` fallback is live and therefore cannot trigger scene-motion +replanning. Supplying `object_initial_pose` disables this implicit semantic +dependency because the explicit pose value is authoritative. Both bound end-effector profiles must provide `open` and `grasp`. Important `CoordinatedPickmentOptions` fields group into: @@ -549,9 +596,11 @@ The middle and final poses are currently option tensors rather than `SceneEntityPose` goal fields. Consequently, handover supports tracking-error and timeout recovery, but does not automatically invalidate a moving handover point. An application can submit a newer invocation revision with updated -`HandOverOptions`; the action also queries the semantic object's live -orientation when replanning and preserves it at the supplied middle/final -positions. +`HandOverOptions`. The action verifies that the goal and source attachment have +the same stable object identity, then derives the current object orientation +from the observed source EEF pose and verified `object_to_eef` relation. +The reused `GraspGoal.grasp_xpos` field is not consumed by `HandOver` and does +not create a scene dependency. As with the other coordinated primitive, cuRobo does not currently support its dual-arm `strategy="motion_gen"` path. diff --git a/docs/source/overview/sim/atomic_actions/index.md b/docs/source/overview/sim/atomic_actions/index.md index 6b38c9164..c4b2f7ab2 100644 --- a/docs/source/overview/sim/atomic_actions/index.md +++ b/docs/source/overview/sim/atomic_actions/index.md @@ -366,6 +366,10 @@ The similarly named `AtomicAction.plan()` method is not a fourth application entry point. It is a framework-owned template method called by the engine after resolving an invocation; skill implementations provide `_plan()`: +This is a deliberate hard extension boundary. Defining `plan()` on a subclass +raises `TypeError` at class definition and has no compatibility adapter. Migrate +an older custom action by renaming its implementation to `_plan()`. + | API | Intended caller | Behavior | |---|---|---| | `AtomicAction.plan(request, context)` | `AtomicActionEngine` | Binds the current collision scene into a copied policy, then delegates to `_plan()` | @@ -624,10 +628,11 @@ resets the new revision's local recovery counters, emits ```{attention} Automatic dynamic-goal invalidation is dependency-driven. A goal must contain a -`SceneEntityPose` for the session to track that scene entity. A primitive that -directly queries a simulation entity during planning will use its latest pose -when planning happens, but that query alone does not trigger scene-motion -replanning. +`SceneEntityPose`, or an object-centric primitive must explicitly declare the +`ObjectSemantics.entity_id` whose snapshot pose it consumes. `PickUp` and the +implicit-initial-pose path of coordinated pickup declare that dependency +automatically. The deprecated live-entity fallback does not trigger +scene-motion replanning. Dynamic collision invalidation is provider-driven. Only registered, pose-updatable collision entities are supported; adding/removing obstacles or @@ -696,7 +701,8 @@ A new primitive should: 4. put reusable embodiment commands on control-part profiles and generic motion/recovery choices in invocation policies; 5. implement side-effect-free `_plan(request, context)` using the engine-owned - planning services; do not override the framework-owned public `plan()`; + planning services; do not override the framework-owned public `plan()`—the + class definition is rejected if it does; 6. return full-robot timed motion, per-environment planning success, optional named segment metadata, diagnostics, and uncommitted effects; 7. add registration coverage, contract tests, execution/recovery tests, a diff --git a/docs/source/overview/sim/planners/curobo_planner.md b/docs/source/overview/sim/planners/curobo_planner.md index 915f7f098..24f3193c1 100644 --- a/docs/source/overview/sim/planners/curobo_planner.md +++ b/docs/source/overview/sim/planners/curobo_planner.md @@ -150,6 +150,12 @@ or live in an offset base frame, also declare their names in `"cuboid"` or `"mesh"` representation because sphere fitting expands one object into multiple independently named obstacles. +`CuroboWorldCfg` validates this planner-local registration at construction: +obstacle names must be unique, and every dynamic obstacle name must match the +`uid` (or generated fallback name) of an entry in `rigid_objects`. The later +`SceneRegistry` integration additionally cross-validates those names with the +scene provider rather than duplicating them in task code. + ### Shared and per-environment collision worlds `CuroboWorldCfg.multi_env` controls collision-world batching only. Robot start diff --git a/docs/source/tutorial/atomic_actions.rst b/docs/source/tutorial/atomic_actions.rst index f9d57c009..1bba99004 100644 --- a/docs/source/tutorial/atomic_actions.rst +++ b/docs/source/tutorial/atomic_actions.rst @@ -85,6 +85,11 @@ the protected ``_plan()`` hook instead. Similarly, ``engine.plan_action()`` is reserved for extensions and isolated tests that need to plan an unregistered instance. +This extension contract is intentionally strict: a subclass that defines +``plan()`` raises ``TypeError`` at class definition. There is no legacy adapter; +custom actions must rename that implementation to ``_plan()`` so the +framework-owned collision-scene preparation cannot be bypassed. + Runnable examples ----------------- @@ -361,10 +366,12 @@ The session replans from its latest context and emits an ``invocation_revised`` event. ``skill_id`` and ``invocation_id`` must still identify the active logical call. -Only entities referenced through ``SceneEntityPose`` become automatic -scene-motion dependencies. A skill may query a simulation entity's live pose -when it plans, but that query alone does not cause an executing session to -replan when the entity moves. +Entities referenced through ``SceneEntityPose`` become automatic scene-motion +dependencies. Object-centric skills may additionally declare an explicit +``ObjectSemantics.entity_id`` when they ground an object pose from the same +scene snapshot; for example, ``PickUp`` automatically tracks that ID. The +legacy ``ObjectSemantics.entity`` live-pose fallback is deprecated and does not +create a scene dependency. Task-state effects ------------------ @@ -397,7 +404,8 @@ Define an action-owned frozen goal dataclass with a stable ``goal_kind``. Then define typed runtime options when needed, implement the protected ``_plan(request, context)`` hook, and declare the stable skill metadata. Do not override the inherited public ``plan()`` method because it binds the latest -collision scene first. +collision scene first. Legacy custom actions that implemented ``plan()`` must +rename it to ``_plan()``; defining ``plan()`` is rejected immediately. Return scalar or per-environment planner success through ``build_plan``. The framework normalizes the mask and holds failed rows at the observed qpos, so a diff --git a/embodichain/lab/sim/atomic_actions/affordance.py b/embodichain/lab/sim/atomic_actions/affordance.py index fdab5e91f..dbe1ffea7 100644 --- a/embodichain/lab/sim/atomic_actions/affordance.py +++ b/embodichain/lab/sim/atomic_actions/affordance.py @@ -240,17 +240,18 @@ def get_approach_direction(self, point_idx: int) -> torch.Tensor: class AssembleAffordance(Affordance): """Affordance describing how an assemble object fits onto a base object. - The base object anchors the assembly: its world pose is read at planning - time from :attr:`base_object_entity` so the target tracks a moved base. The - assemble object is the part that is picked up and placed; its target pose is - ``base_pose @ assemble_to_base_pose``. + The affordance stores the relative assembly relation. Canonical planning + supplies the base object's snapshot pose through ``AssembleGoal.base_pose``; + :attr:`base_object_entity` is retained only as a deprecated direct-core + fallback when that goal field is omitted. The assemble object's target pose + is ``base_pose @ assemble_to_base_pose``. """ base_object_label: str = "" """Label of the base object the assemble object is placed onto.""" base_object_entity: BatchEntity | None = None - """Simulation entity for the base object; its pose anchors the assembly.""" + """Legacy live base entity used only when ``AssembleGoal.base_pose`` is absent.""" assemble_object_label: str = "" """Label of the assemble object that is picked up and placed.""" @@ -274,18 +275,41 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor: Returns: Assemble-object target pose with shape ``(n_envs, 4, 4)``. + + Raises: + TypeError: If either pose value is not a tensor. + ValueError: If either pose has an unsupported shape or batch size. """ + if not isinstance(base_pose, torch.Tensor): + raise TypeError("base_pose must be a torch.Tensor.") base_pose = base_pose.to(dtype=torch.float32) - if base_pose.dim() == 2: + if base_pose.shape == (4, 4): base_pose = base_pose.unsqueeze(0) + elif ( + base_pose.dim() != 3 + or base_pose.shape[0] == 0 + or base_pose.shape[-2:] != (4, 4) + ): + raise ValueError("base_pose must have shape (4, 4) or (n_envs, 4, 4).") n_envs = base_pose.shape[0] + if not isinstance(self.assemble_to_base_pose, torch.Tensor): + raise TypeError("assemble_to_base_pose must be a torch.Tensor.") rel = self.assemble_to_base_pose.to( device=base_pose.device, dtype=torch.float32 ) - if rel.dim() == 2: + if rel.shape == (4, 4): rel = rel.unsqueeze(0).repeat(n_envs, 1, 1) + elif rel.dim() != 3 or rel.shape[-2:] != (4, 4) or rel.shape[0] == 0: + raise ValueError( + "assemble_to_base_pose must have shape (4, 4), (1, 4, 4), " + "or (n_envs, 4, 4)." + ) elif rel.shape[0] == 1: rel = rel.repeat(n_envs, 1, 1) + elif rel.shape[0] != n_envs: + raise ValueError( + "assemble_to_base_pose batch size must match base_pose batch size." + ) return torch.bmm(base_pose, rel) diff --git a/embodichain/lab/sim/atomic_actions/core.py b/embodichain/lab/sim/atomic_actions/core.py index ee3b1f391..d434cc000 100644 --- a/embodichain/lab/sim/atomic_actions/core.py +++ b/embodichain/lab/sim/atomic_actions/core.py @@ -70,9 +70,15 @@ def resolve_runtime_device(device: torch.device | str) -> torch.device: return resolved -@dataclass +@dataclass(frozen=True, slots=True, eq=False) class ObjectSemantics: - """Semantic and geometric information about an interaction object.""" + """Shallow-frozen semantic information about an interaction object. + + .. attention:: + Top-level fields cannot be rebound after construction. Nested + affordance and metadata objects may remain mutable but never establish + object identity. + """ affordance: Affordance """Affordance data describing supported interactions.""" @@ -89,6 +95,9 @@ class ObjectSemantics: entity: BatchEntity | None = None """Optional simulation entity used by deterministic grounding.""" + entity_id: str | None = None + """Stable scene identifier used by snapshot grounding and explicit identity.""" + def __post_init__(self) -> None: if not isinstance(self.affordance, Affordance): raise TypeError("affordance must be an Affordance instance.") @@ -98,9 +107,39 @@ def __post_init__(self) -> None: raise TypeError("properties must be a dict.") if not isinstance(self.label, str) or not self.label: raise ValueError("label must be a non-empty string.") + if self.entity_id is not None and ( + not isinstance(self.entity_id, str) or not self.entity_id.strip() + ): + raise ValueError("entity_id must be a non-empty string when set.") self.affordance.object_label = self.label +def _legacy_object_uid(semantics: ObjectSemantics) -> str | None: + """Return a valid legacy simulation UID without alias normalization.""" + uid = getattr(semantics.entity, "uid", None) + return uid if isinstance(uid, str) and uid.strip() else None + + +def _same_object_identity( + left: ObjectSemantics, + right: ObjectSemantics, +) -> bool: + """Return whether two semantic snapshots identify the same object.""" + if left is right: + return True + if left.entity_id is not None or right.entity_id is not None: + return ( + left.entity_id is not None + and right.entity_id is not None + and left.entity_id == right.entity_id + ) + left_uid = _legacy_object_uid(left) + right_uid = _legacy_object_uid(right) + if left_uid is not None or right_uid is not None: + return left_uid is not None and right_uid is not None and left_uid == right_uid + return left.entity is not None and left.entity is right.entity + + @dataclass(frozen=True, slots=True) class SkillDescriptor: """Machine-readable metadata for one registered atomic skill.""" @@ -429,6 +468,13 @@ def _uses_collision_world( ) return available + def _scene_dependencies( + self, + request: ResolvedActionRequest[GoalT, OptionsT], + ) -> tuple[str, ...]: + """Return scene entities whose poses materially affect this plan.""" + return collect_scene_dependencies(request.goal) + def build_plan( self, request: ResolvedActionRequest[GoalT, OptionsT], @@ -522,7 +568,7 @@ def build_plan( ), diagnostics=diagnostics, segments=tuple(segments), - scene_dependencies=collect_scene_dependencies(request.goal), + scene_dependencies=self._scene_dependencies(request), collision_world_sensitive=self._uses_collision_world( request, context, diff --git a/embodichain/lab/sim/atomic_actions/effects.py b/embodichain/lab/sim/atomic_actions/effects.py index f9c6507aa..f9c1f537b 100644 --- a/embodichain/lab/sim/atomic_actions/effects.py +++ b/embodichain/lab/sim/atomic_actions/effects.py @@ -68,6 +68,8 @@ def _merge_held( update_mask: torch.Tensor, ) -> HeldObjectState | None: """Apply one optional held-object update per environment.""" + from .core import _same_object_identity + if previous is None and candidate is None: return None if previous is None: @@ -85,7 +87,7 @@ def _merge_held( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different held-object semantics for one resource " @@ -96,7 +98,7 @@ def _merge_held( return None selector = update_mask[:, None, None] return HeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + semantics=(previous.semantics if previous_retained else candidate.semantics), object_to_eef=torch.where( selector, candidate.object_to_eef, previous.object_to_eef ), @@ -111,6 +113,8 @@ def _merge_coordinated( update_mask: torch.Tensor, ) -> CoordinatedHeldObjectState | None: """Apply one optional coordinated relation update per environment.""" + from .core import _same_object_identity + if previous is None and candidate is None: return None if previous is None: @@ -128,7 +132,7 @@ def _merge_coordinated( if ( previous_retained and candidate_applied - and previous.semantics is not candidate.semantics + and not _same_object_identity(previous.semantics, candidate.semantics) ): raise ValueError( "Cannot merge different coordinated held-object semantics for one " @@ -139,7 +143,7 @@ def _merge_coordinated( return None selector = update_mask[:, None, None] return CoordinatedHeldObjectState( - semantics=candidate.semantics if candidate_applied else previous.semantics, + semantics=(previous.semantics if previous_retained else candidate.semantics), left_object_to_eef=torch.where( selector, candidate.left_object_to_eef, previous.left_object_to_eef ), diff --git a/embodichain/lab/sim/atomic_actions/goals.py b/embodichain/lab/sim/atomic_actions/goals.py index bdf38811a..f8d031130 100644 --- a/embodichain/lab/sim/atomic_actions/goals.py +++ b/embodichain/lab/sim/atomic_actions/goals.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from collections.abc import Mapping, Sequence from dataclasses import dataclass, fields, is_dataclass from typing import Any, ClassVar, Protocol, TYPE_CHECKING @@ -163,13 +164,55 @@ def resolve_pose_goal( return torch.bmm(pose, relative) +def _resolve_object_pose( + semantics: ObjectSemantics, + context: PlanningContext, + *, + name: str = "object", +) -> torch.Tensor: + """Resolve an object's pose from a snapshot or the deprecated live handle.""" + from .core import ObjectSemantics + + if not isinstance(semantics, ObjectSemantics): + raise TypeError("semantics must be an ObjectSemantics instance.") + if semantics.entity_id is not None: + return resolve_pose_goal( + SceneEntityPose(semantics.entity_id), + context, + name=name, + ) + if semantics.entity is None: + raise ValueError( + f"{name} requires ObjectSemantics.entity_id or a legacy entity handle." + ) + warnings.warn( + "Live pose grounding through ObjectSemantics.entity is deprecated; " + "set entity_id and provide the entity through PlanningContext.scene.", + DeprecationWarning, + stacklevel=2, + ) + pose = semantics.entity.get_local_pose(to_matrix=True) + if not isinstance(pose, torch.Tensor): + raise TypeError(f"{name} legacy entity pose must be a torch.Tensor.") + pose = pose.to(device=context.robot.qpos.device, dtype=torch.float32) + if pose.shape == (4, 4): + pose = pose.unsqueeze(0).expand(context.batch_size, -1, -1) + elif pose.shape != (context.batch_size, 4, 4): + raise ValueError(f"{name} legacy entity pose must match planning batch size.") + return pose.clone() + + def collect_scene_dependencies(value: Any) -> tuple[str, ...]: """Collect stable scene entity identifiers referenced by a goal value.""" + from .core import ObjectSemantics + found: set[str] = set() def visit(item: Any) -> None: if isinstance(item, SceneEntityPose): found.add(item.entity_id) + elif isinstance(item, ObjectSemantics): + return elif is_dataclass(item) and not isinstance(item, type): for data_field in fields(item): visit(getattr(item, data_field.name)) diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index f59d5286c..c5d7900b0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -34,6 +34,7 @@ from ..goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, resolve_pose_goal, validate_pose_goal, ) @@ -59,7 +60,11 @@ class CoordinatedPickGoal(ObjectActionGoal): """Target pose for the shared object, shape ``(4, 4)`` or ``(n_envs, 4, 4)``.""" object_initial_pose: PoseGoalValue | None = None - """Optional initial object pose. Defaults to ``semantics.entity`` pose.""" + """Optional initial object pose. + + When omitted, the pose is grounded through the semantic object's stable + scene identity, with its live entity retained only as a legacy fallback. + """ def __post_init__(self) -> None: ObjectActionGoal.__post_init__(self) @@ -359,6 +364,22 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[ + CoordinatedPickGoal, + CoordinatedPickmentOptions, + ], + ) -> tuple[str, ...]: + """Track the semantic object only when it supplies the initial pose.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if target.object_initial_pose is None: + entity_id = target.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + return tuple(sorted(dependencies)) + def _resolve_resources( self, request: ResolvedActionRequest[CoordinatedPickGoal, CoordinatedPickmentOptions], @@ -424,14 +445,12 @@ def _resolve_object_initial_pose( ), "object_initial_pose", ) - if target.semantics.entity is None: - logger.log_error( - "CoordinatedPickGoal requires object_initial_pose when " - "semantics.entity is not provided.", - ValueError, - ) return self._resolve_pose( - target.semantics.entity.get_local_pose(to_matrix=True), + _resolve_object_pose( + target.semantics, + context, + name="object_initial_pose", + ), "object_initial_pose", ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index 4c0878773..120c8b5d0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -28,7 +28,7 @@ from ..bindings import ResolvedControlPart from ..control import GRASP_COMMAND, OPEN_COMMAND -from ..core import AtomicAction, ObjectSemantics +from ..core import AtomicAction, ObjectSemantics, _same_object_identity from ..effects import StateDelta from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask @@ -149,6 +149,14 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[GraspGoal, HandOverOptions], + ) -> tuple[str, ...]: + """Return no goal-pose dependency because handover ignores grasp_xpos.""" + del request + return () + def _resolve_resources( self, request: ResolvedActionRequest[GraspGoal, HandOverOptions], @@ -224,7 +232,13 @@ def _plan( state = context semantics = target.semantics transfer_object_to_eef = self._resolve_transfer_object_to_eef( - state, resources.transfer_arm.name + state, + resources.transfer_arm.name, + semantics, + ) + transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( + state, + resources, ) assert options.middle_object_pose is not None assert options.final_object_pose is not None @@ -241,8 +255,17 @@ def _plan( receive_approach_direction / torch.linalg.vector_norm(receive_approach_direction) ) - # force object pose to have the same rotation as the current object pose, so that the handover is feasible. - current_object_pose = target.semantics.entity.get_local_pose(to_matrix=True) + # Keep the requested object orientation consistent with the verified + # attachment and the transferring arm's current measured pose. + transfer_current_eef = self.robot.compute_fk( + qpos=transfer_start_qpos, + name=resources.transfer_arm.name, + to_matrix=True, + ) + current_object_pose = torch.bmm( + transfer_current_eef, + pose_inv(transfer_object_to_eef), + ) middle_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] final_object_pose[:, :3, :3] = current_object_pose[:, :3, :3] @@ -285,9 +308,6 @@ def _plan( ), ) - transfer_start_qpos, receive_start_qpos = self._resolve_start_qpos( - state, resources - ) segments = self._compute_segment_lengths( request.motion_policy.sample_count, options ) @@ -507,7 +527,7 @@ def _validate_pose_options(options: HandOverOptions) -> None: ) def _resolve_matrix(self, matrix: torch.Tensor, name: str) -> torch.Tensor: - matrix = matrix.to(device=self.device, dtype=torch.float32) + matrix = matrix.to(device=self.device, dtype=torch.float32).clone() if matrix.shape == (4, 4): matrix = matrix.unsqueeze(0).repeat(self.n_envs, 1, 1) if matrix.shape != (self.n_envs, 4, 4): @@ -522,6 +542,7 @@ def _resolve_transfer_object_to_eef( self, state: PlanningContext, transfer_control_part: str, + target_semantics: ObjectSemantics, ) -> torch.Tensor: held = state.get_held_object(transfer_control_part) if held is None: @@ -530,6 +551,11 @@ def _resolve_transfer_object_to_eef( f"{transfer_control_part!r} (run PickUp first).", ValueError, ) + if not _same_object_identity(target_semantics, held.semantics): + raise ValueError( + "HandOver target semantics must identify the object held by " + f"transfer control part {transfer_control_part!r}." + ) return self._resolve_matrix(held.object_to_eef, "held_object.object_to_eef") def _resolve_receive_grasp( diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py index 6bf8743b5..917a758c7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -24,7 +24,11 @@ import torch from embodichain.utils import logger -from embodichain.utils.math import axis_angle_to_rotation_matrix, get_relative_rotation +from embodichain.utils.math import ( + axis_angle_to_rotation_matrix, + get_relative_rotation, + pose_inv, +) from ._helpers import arm_qpos_from_state, resolve_object_target from ..control import GRASP_COMMAND @@ -138,18 +142,19 @@ def _plan( end_arm_xpos = self.robot.compute_fk( start_arm_qpos, name=control_part, to_matrix=True ) + object_to_eef = held_object.object_to_eef.to( + device=self.device, dtype=torch.float32 + ) + if object_to_eef.shape == (4, 4): + object_to_eef = object_to_eef.unsqueeze(0).repeat(self.n_envs, 1, 1) + current_object_pose = torch.bmm(end_arm_xpos, pose_inv(object_to_eef)) if options.pick_rotate_upright is not None: self._apply_configured_upright_rotation( object_target_pose, end_arm_xpos, - held_object.semantics.entity.get_local_pose(to_matrix=True), + current_object_pose, options, ) - object_to_eef = held_object.object_to_eef.to( - device=self.device, dtype=torch.float32 - ) - if object_to_eef.shape == (4, 4): - object_to_eef = object_to_eef.unsqueeze(0).repeat(self.n_envs, 1, 1) move_eef_xpos = torch.bmm(object_target_pose, object_to_eef) if options.pick_rotate_upright is None: diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index ed81535c6..89832e50a 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -41,6 +41,7 @@ from ..goals import ( ObjectActionGoal, PoseGoalValue, + _resolve_object_pose, resolve_pose_goal, validate_pose_goal, ) @@ -166,6 +167,17 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[GraspGoal, PickUpOptions], + ) -> tuple[str, ...]: + """Include the semantic object when it has a stable scene identity.""" + dependencies = set(super()._scene_dependencies(request)) + entity_id = request.goal.semantics.entity_id + if entity_id is not None: + dependencies.add(entity_id) + return tuple(sorted(dependencies)) + def _get_full_pickup_trajectory( self, grasp_xpos: torch.Tensor, @@ -289,6 +301,11 @@ def _plan( control_part = manipulator.name state = context sem = target.semantics + object_pose = _resolve_object_pose( + sem, + context, + name="pickup_object_pose", + ) if target.grasp_xpos is None and not isinstance( sem.affordance, AntipodalAffordance ): @@ -296,17 +313,18 @@ def _plan( "PickUp requires an AntipodalAffordance when grasp_xpos is not set.", ValueError, ) - if sem.entity is None: - logger.log_error( - "PickUp requires an entity on the target semantics.", ValueError - ) start_arm_qpos = arm_qpos_from_state( state, list(manipulator.joint_ids), ) if target.grasp_xpos is None: is_success, grasp_xpos = self._resolve_grasp_pose( - sem, start_arm_qpos, manipulator, options, approach_direction + sem, + object_pose, + start_arm_qpos, + manipulator, + options, + approach_direction, ) else: grasp_xpos = resolve_pose_target( @@ -316,7 +334,9 @@ def _plan( ) if options.rotate_upright is not None: grasp_xpos = self._upright_adjusted_grasp_poses( - sem, grasp_xpos, options + grasp_xpos, + object_pose, + options, ) is_success = torch.ones(self.n_envs, dtype=torch.bool, device=self.device) grasp_success = normalize_success_mask( @@ -350,8 +370,7 @@ def _plan( name="Pick-up trajectory success", ) - obj_poses = sem.entity.get_local_pose(to_matrix=True) - object_to_eef = torch.bmm(pose_inv(obj_poses), grasp_xpos) + object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) @@ -373,18 +392,18 @@ def _plan( def _resolve_grasp_pose( self, semantics: ObjectSemantics, + object_pose: torch.Tensor, start_qpos: torch.Tensor, manipulator: ResolvedControlPart, options: PickUpOptions, approach_direction: torch.Tensor, ) -> tuple[torch.Tensor, torch.Tensor]: - obj_poses = semantics.entity.get_local_pose(to_matrix=True) grasp_poses_result = semantics.affordance.get_valid_grasp_poses( - obj_poses=obj_poses, + obj_poses=object_pose, approach_direction=approach_direction, object_part=options.pick_object_part, ) - n_envs = obj_poses.shape[0] + n_envs = object_pose.shape[0] n_max_pose = max(r[0].shape[0] for r in grasp_poses_result) grasp_xpos_padding = torch.zeros( (n_envs, n_max_pose, 4, 4), dtype=torch.float32, device=self.device @@ -408,10 +427,9 @@ def _resolve_grasp_pose( grasp_xpos_padding[i, n_pose:] = grasp_poses[0] grasp_cost_padding[i, n_pose:] = grasp_costs[0] grasp_xpos_padding, ik_success = self._select_feasible_grasp_variants( - semantics, grasp_xpos_padding, start_qpos, - obj_poses, + object_pose, manipulator, options, approach_direction, @@ -426,7 +444,6 @@ def _resolve_grasp_pose( def _select_feasible_grasp_variants( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, start_qpos: torch.Tensor, object_poses: torch.Tensor, @@ -441,7 +458,9 @@ def _select_feasible_grasp_variants( mirrored_grasp_xpos[..., :3, 1] = -mirrored_grasp_xpos[..., :3, 1] selection_variants = torch.stack([grasp_xpos, mirrored_grasp_xpos], dim=2) grasp_variants = self._upright_adjusted_grasp_poses( - semantics, selection_variants, options + selection_variants, + object_poses, + options, ) pre_grasp_variants = grasp_variants.clone() @@ -576,8 +595,8 @@ def _compute_batch_candidate_ik( def _upright_adjusted_grasp_poses( self, - semantics: ObjectSemantics, grasp_xpos: torch.Tensor, + object_pose: torch.Tensor, options: PickUpOptions, ) -> torch.Tensor: """Return grasp poses after the optional upright-in-place roll adjustment.""" @@ -592,8 +611,7 @@ def _upright_adjusted_grasp_poses( upright_direction = options.obj_upright_direction.to( device=self.device, dtype=torch.float32 ) - obj_pose = semantics.entity.get_local_pose(to_matrix=True) - obj_upright = torch.matmul(obj_pose[:, :3, :3], upright_direction) + obj_upright = torch.matmul(object_pose[:, :3, :3], upright_direction) adjusted_grasp_xpos = grasp_xpos.clone() grasp_ry = adjusted_grasp_xpos[..., :3, 1] object_axes = obj_upright.reshape( diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index e809c0a2a..7218a5d27 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -18,6 +18,7 @@ from __future__ import annotations +import warnings from dataclasses import dataclass from typing import ClassVar, Literal @@ -31,7 +32,12 @@ from ..control import GRASP_COMMAND, OPEN_COMMAND from ..core import AtomicAction from ..effects import StateDelta -from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal +from ..goals import ( + PoseGoalValue, + SceneEntityPose, + resolve_pose_goal, + validate_pose_goal, +) from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..state import PlanningContext @@ -79,11 +85,12 @@ def __post_init__(self) -> None: class AssembleGoal: """Place a held assemble object onto a base object at a relative pose. - The base object pose is read at planning time from - :attr:`AssembleAffordance.base_object_entity`, and the assemble object's - target pose is ``base_pose @ assemble_to_base_pose``. The held-object - transform (``object_to_eef``) is read from :class:`PlanningContext` - for the place control part, which a prior :class:`PickUp` populates. + The preferred base object pose is a late-bound :class:`SceneEntityPose`. + Omitting it temporarily falls back to + :attr:`AssembleAffordance.base_object_entity`. The assemble object's target + pose is ``base_pose @ assemble_to_base_pose``. The held-object transform + (``object_to_eef``) is read from :class:`PlanningContext` for the place + control part, which a prior :class:`PickUp` populates. """ goal_kind: ClassVar[str] = "assemble" @@ -91,6 +98,18 @@ class AssembleGoal: affordance: AssembleAffordance """Assembly affordance anchoring the assemble object to the base object.""" + base_pose: SceneEntityPose | None = None + """Late-bound base-object pose used for snapshot-consistent planning.""" + + def __post_init__(self) -> None: + if not isinstance(self.affordance, AssembleAffordance): + raise TypeError("affordance must be an AssembleAffordance instance.") + if self.base_pose is not None and not isinstance( + self.base_pose, + SceneEntityPose, + ): + raise TypeError("base_pose must be a SceneEntityPose or None.") + @dataclass(frozen=True, slots=True, eq=False) class PlaceOptions(ActionOptions): @@ -129,9 +148,10 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): joint positions are inherited from :class:`PlanningContext`. An :class:`AssembleGoal` replaces the explicit EEF pose with an assembly - affordance: the place pose is derived from the base object's current pose - and ``assemble_to_base_pose``, converted to an EEF pose through the held - object's ``object_to_eef`` (read from :class:`PlanningContext`). + affordance: the place pose is derived from the base object's snapshot pose + (or deprecated live fallback) and ``assemble_to_base_pose``, converted to an + EEF pose through the held object's ``object_to_eef`` (read from + :class:`PlanningContext`). """ skill_id: ClassVar[str] = "place" @@ -154,6 +174,17 @@ def _on_bind(self) -> None: self.n_envs = self.robot.get_qpos().shape[0] self.robot_dof = self.robot.dof + def _scene_dependencies( + self, + request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], + ) -> tuple[str, ...]: + """Include an explicitly snapshot-grounded assembly base.""" + dependencies = set(super()._scene_dependencies(request)) + target = request.goal + if isinstance(target, AssembleGoal) and target.base_pose is not None: + dependencies.add(target.base_pose.entity_id) + return tuple(sorted(dependencies)) + def _plan( self, request: ResolvedActionRequest[PlaceGoal | AssembleGoal, PlaceOptions], @@ -325,7 +356,7 @@ def _resolve_assemble_place_xpos( Place EEF poses with shape ``(n_envs, 4, 4)``. Raises: - ValueError: If no held object or no base object entity is available. + ValueError: If no held object or base-pose source is available. """ held = state.get_held_object(control_part) if held is None: @@ -335,15 +366,37 @@ def _resolve_assemble_place_xpos( ValueError, ) affordance = target.affordance - if affordance.base_object_entity is None: - logger.log_error( - "AssembleAffordance.base_object_entity must be set to assemble " - "onto a base object.", - ValueError, + if target.base_pose is not None: + base_pose = resolve_object_target( + resolve_pose_goal( + target.base_pose, + state, + name="base_pose", + ), + n_envs=self.n_envs, + device=self.device, + name="base_pose", + ) + else: + if affordance.base_object_entity is None: + logger.log_error( + "AssembleGoal requires base_pose or " + "AssembleAffordance.base_object_entity.", + ValueError, + ) + warnings.warn( + "AssembleGoal without base_pose reads " + "AssembleAffordance.base_object_entity live; provide " + "base_pose=SceneEntityPose(...) instead.", + DeprecationWarning, + stacklevel=3, + ) + base_pose = resolve_object_target( + affordance.base_object_entity.get_local_pose(to_matrix=True), + n_envs=self.n_envs, + device=self.device, + name="legacy_base_pose", ) - base_pose = affordance.base_object_entity.get_local_pose(to_matrix=True).to( - device=self.device, dtype=torch.float32 - ) assemble_object_pose = affordance.get_assemble_object_pose(base_pose) object_to_eef = resolve_object_target( held.object_to_eef, diff --git a/embodichain/lab/sim/atomic_actions/trajectory_ops.py b/embodichain/lab/sim/atomic_actions/trajectory_ops.py index 0154e8418..ca4d4c61a 100644 --- a/embodichain/lab/sim/atomic_actions/trajectory_ops.py +++ b/embodichain/lab/sim/atomic_actions/trajectory_ops.py @@ -18,7 +18,6 @@ from __future__ import annotations -import numpy as np import torch from embodichain.lab.sim.planners import MoveType, PlanResult, PlanState @@ -182,7 +181,7 @@ def split_three_segments( third_segment_name: str = "third", ) -> tuple[int, int, int]: """Split a sample budget into motion, hand, and motion segments.""" - first = int(np.round(sample_count - hand_interp_steps) * first_segment_ratio) + first = int(round((sample_count - hand_interp_steps) * first_segment_ratio)) if first < 2: raise ValueError( f"Not enough waypoints for {first_segment_name} trajectory. " diff --git a/embodichain/lab/sim/planners/curobo/curobo_planner.py b/embodichain/lab/sim/planners/curobo/curobo_planner.py index f9cf5fce5..51766737f 100644 --- a/embodichain/lab/sim/planners/curobo/curobo_planner.py +++ b/embodichain/lab/sim/planners/curobo/curobo_planner.py @@ -178,7 +178,7 @@ class CuroboWorldCfg: """ dynamic_obstacle_names: list[str] = [] - """Obstacle names whose poses may be updated between plans.""" + """Registered rigid-object names whose poses may be updated between plans.""" multi_env: bool = False """Whether cuRobo allocates one collision-world instance per environment. @@ -211,6 +211,35 @@ class CuroboWorldCfg: """ def __post_init__(self) -> None: + dynamic_names = list(self.dynamic_obstacle_names) + if len(set(dynamic_names)) != len(dynamic_names) or not all( + isinstance(name, str) and name for name in dynamic_names + ): + raise ValueError( + "dynamic_obstacle_names must contain unique non-empty names." + ) + + rigid_objects = list(self.rigid_objects or ()) + rigid_names = [ + getattr(obj, "uid", None) or f"obstacle_{index}" + for index, obj in enumerate(rigid_objects) + ] + if not all(isinstance(name, str) and name for name in rigid_names): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have non-empty string names." + ) + if len(set(rigid_names)) != len(rigid_names): + raise ValueError( + "CuroboWorldCfg.rigid_objects must have unique obstacle names." + ) + missing = set(dynamic_names).difference(rigid_names) + if missing: + raise ValueError( + "dynamic_obstacle_names reference objects not present in " + f"rigid_objects: {sorted(missing)}." + ) + self.dynamic_obstacle_names = dynamic_names + # Wrap live RigidObjects so the @configclass field-deepcopy (run right # after this by custom_post_init) shares references instead of trying to # pickle non-pickleable C++ dexsim handles held by each RigidObject. diff --git a/embodichain/lab/sim/planners/utils.py b/embodichain/lab/sim/planners/utils.py index 1913449eb..76a4e1beb 100644 --- a/embodichain/lab/sim/planners/utils.py +++ b/embodichain/lab/sim/planners/utils.py @@ -58,11 +58,18 @@ def normalize_success_mask( Raises: TypeError: If ``success`` is neither boolean nor binary integer data. - ValueError: If a tensor does not match the required batch shape. + ValueError: If a tensor does not match the required batch shape or a + CUDA device is requested while CUDA is unavailable. """ resolved_device = torch.device(device) - if resolved_device.type == "cuda" and resolved_device.index is None: - resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") + if resolved_device.type == "cuda": + if not torch.cuda.is_available(): + raise ValueError( + "CUDA device requested for success-mask normalization, but " + "torch.cuda.is_available() is False." + ) + if resolved_device.index is None: + resolved_device = torch.device(f"cuda:{torch.cuda.current_device()}") if isinstance(success, bool): return torch.full((n_envs,), success, dtype=torch.bool, device=resolved_device) if not isinstance(success, torch.Tensor): diff --git a/scripts/tutorials/atomic_action/moving_target_recovery.py b/scripts/tutorials/atomic_action/moving_target_recovery.py index a9053453d..a34efec07 100644 --- a/scripts/tutorials/atomic_action/moving_target_recovery.py +++ b/scripts/tutorials/atomic_action/moving_target_recovery.py @@ -275,6 +275,7 @@ def main() -> None: geometry={}, label="cube", entity=target, + entity_id=TARGET_ENTITY_ID, ) binding = ActionBinding( manipulators={"primary": "arm"}, diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 06703fae5..46384558b 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -29,6 +29,7 @@ ActionInvocation, Affordance, AntipodalAffordance, + AssembleAffordance, AssembleGoal, AtomicAction, AtomicActionEngine, @@ -77,6 +78,7 @@ PlanOptions, PlanResult, ) +from embodichain.utils.math import pose_inv NUM_ENVS = 2 ARM_DOF = 6 @@ -269,7 +271,7 @@ def _invocation( ) -def _semantics() -> ObjectSemantics: +def _semantics(*, entity_id: str | None = None) -> ObjectSemantics: entity = Mock() entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) return ObjectSemantics( @@ -277,6 +279,7 @@ def _semantics() -> ObjectSemantics: geometry={}, label="test_object", entity=entity, + entity_id=entity_id, ) @@ -362,12 +365,16 @@ def compute_fk( return generator -def _dual_context(task: TaskState | None = None) -> PlanningContext: +def _dual_context( + task: TaskState | None = None, + *, + scene: SceneSnapshot | None = None, +) -> PlanningContext: qpos = torch.zeros(NUM_ENVS, DUAL_ROBOT_DOF) return PlanningContext( robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), task=task or TaskState.empty(NUM_ENVS, "cpu"), - scene=SceneSnapshot.empty(), + scene=SceneSnapshot.empty() if scene is None else scene, env_ids=torch.arange(NUM_ENVS), ) @@ -513,8 +520,9 @@ def test_move_joints_uses_binding_and_preserves_uncontrolled_joints() -> None: def test_pick_and_place_declare_effects_without_mutating_context() -> None: generator = _motion_generator() pick = _bind_action(generator, PickUp()) - initial = _context() - semantics = _semantics() + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + initial = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + semantics = _semantics(entity_id="target") grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) pick_plan = _plan_action( @@ -559,15 +567,43 @@ def test_move_held_object_requires_projected_attachment() -> None: with pytest.raises(ValueError, match="requires an object held"): _plan_action(action, invocation, _context()) - held = _held() + semantics = _semantics() + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", held_objects={"arm": held}, ) - plan = _plan_action(action, invocation, _context(task)) + eef_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + eef_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + eef_pose[:, 0, 3] = torch.tensor([0.5, 0.8]) + generator.robot.compute_fk.return_value = eef_pose + generator.robot.compute_fk.side_effect = None + action._apply_configured_upright_rotation = Mock() + configured_invocation = ActionInvocation( + skill_id="move_held_object", + goal=HeldObjectPoseGoal(torch.eye(4)), + binding=_binding(), + motion_policy=MotionPolicy(sample_count=10), + skill_options=MoveHeldObjectOptions(pick_rotate_upright=0.25), + ) + + plan = _plan_action(action, configured_invocation, _context(task)) + assert plan.plan_success.all() assert plan.expected_effects.is_empty + current_object_pose = action._apply_configured_upright_rotation.call_args.args[2] + assert torch.allclose( + current_object_pose, + torch.bmm(eef_pose, pose_inv(held.object_to_eef)), + ) + semantics.entity.get_local_pose.assert_not_called() def test_press_uses_invocation_sample_budget() -> None: @@ -711,32 +747,41 @@ def test_pick_explicit_grasp_bypasses_sampling_and_records_grasp() -> None: affordance = AntipodalAffordance() affordance.get_valid_grasp_poses = Mock() entity = Mock() - entity.get_local_pose.return_value = torch.eye(4).repeat(NUM_ENVS, 1, 1) + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( affordance=affordance, geometry={}, label="explicit-grasp-object", entity=entity, + entity_id="target", ) grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) grasp[:, 0, 3] = torch.tensor([0.1, 0.2]) action = _bind_action(generator, PickUp()) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + object_pose[:, 0, 3] = torch.tensor([0.03, 0.07]) + context = _context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) - plan = _plan_action( - action, + request = action.resolve_request( _invocation( "pick_up", GraspGoal(semantics=semantics, grasp_xpos=grasp), sample_count=20, - ), - _context(), + ) ) - projected = plan.expected_effects.apply(_context().task, plan.plan_success) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) - affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.affordance.get_valid_grasp_poses.assert_not_called() + request.goal.semantics.entity.get_local_pose.assert_not_called() held = projected.get_held_object("arm") assert held is not None assert torch.allclose(held.grasp_xpos, grasp) + assert torch.allclose(held.object_to_eef, torch.bmm(pose_inv(object_pose), grasp)) + assert plan.scene_dependencies == ("target",) assert [segment.name for segment in plan.segments] == [ "approach", "close", @@ -754,6 +799,7 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: geometry={}, label="partially-graspable-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) action._resolve_grasp_pose = Mock( @@ -762,7 +808,13 @@ def test_pick_holds_only_environment_without_a_feasible_grasp() -> None: torch.eye(4).repeat(NUM_ENVS, 1, 1), ) ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) plan = _plan_action( action, @@ -795,6 +847,7 @@ def test_pick_resolves_late_bound_scene_grasp_and_declares_dependency() -> None: geometry={}, label="late-bound-grasp-object", entity=entity, + entity_id="target", ) action = _bind_action(generator, PickUp()) context = _context(scene=_target_scene(target_pose, timestamp=0.0, version=0)) @@ -838,6 +891,7 @@ def test_pick_session_replans_when_late_bound_target_moves() -> None: geometry={}, label="moving-grasp-object", entity=entity, + entity_id="target", ) engine = AtomicActionEngine( generator, @@ -882,16 +936,26 @@ def test_pick_uses_binding_control_part_as_effect_resource() -> None: action = _bind_action(generator, PickUp()) invocation = ActionInvocation( skill_id="pick_up", - goal=GraspGoal(semantics=_semantics(), grasp_xpos=torch.eye(4)), + goal=GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ), binding=ActionBinding( manipulators={"primary": "alternate_arm"}, end_effectors={"primary": "alternate_hand"}, ), motion_policy=MotionPolicy(sample_count=20), ) - context = _context() + context = _context( + scene=_target_scene( + torch.eye(4).repeat(NUM_ENVS, 1, 1), + timestamp=0.0, + version=0, + ) + ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert projected.get_held_object("alternate_arm") is not None @@ -940,15 +1004,24 @@ def test_handover_does_not_mutate_cached_final_pose() -> None: ) assert handover_options.final_object_pose is not None original_final_pose = handover_options.final_object_pose.clone() - current_object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) - current_object_pose[:, :3, :3] = torch.diag(torch.tensor([-1.0, -1.0, 1.0])) - semantics = _semantics() - semantics.entity.get_local_pose.return_value = current_object_pose + semantics = _semantics(entity_id="handover_object") + held = _held(semantics) + held.object_to_eef[:, :3, :3] = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]] + ) + held.object_to_eef[:, 0, 3] = torch.tensor([0.1, 0.2]) task = TaskState( batch_size=NUM_ENVS, device="cpu", - held_objects={"left_arm": _held(semantics)}, + held_objects={"left_arm": held}, ) + current_eef = torch.eye(4).repeat(NUM_ENVS, 1, 1) + current_eef[:, :3, :3] = torch.tensor( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]] + ) + current_eef[:, 1, 3] = torch.tensor([0.3, 0.5]) + generator.robot.compute_fk.return_value = current_eef + generator.robot.compute_fk.side_effect = None receive_grasp = torch.eye(4).repeat(NUM_ENVS, 1, 1) action._resolve_receive_grasp = Mock( return_value=(receive_grasp, torch.ones(NUM_ENVS, dtype=torch.bool)) @@ -966,7 +1039,10 @@ def plan_from_start( action._plan_named_arm_trajectory = Mock(side_effect=plan_from_start) invocation = ActionInvocation( skill_id="hand_over", - goal=GraspGoal(semantics=semantics), + goal=GraspGoal( + semantics=semantics, + grasp_xpos=SceneEntityPose("unused_grasp_pose"), + ), binding=_dual_binding("source", "destination"), motion_policy=MotionPolicy(sample_count=30), ) @@ -974,7 +1050,18 @@ def plan_from_start( plan = _plan_action(action, invocation, _dual_context(task)) assert plan.plan_success.all() + assert plan.scene_dependencies == () + handover_object_pose = action._resolve_receive_grasp.call_args.args[1] + expected_current_object_pose = torch.bmm( + current_eef, + pose_inv(held.object_to_eef), + ) + assert torch.allclose( + handover_object_pose[:, :3, :3], + expected_current_object_pose[:, :3, :3], + ) assert torch.equal(handover_options.final_object_pose, original_final_pose) + semantics.entity.get_local_pose.assert_not_called() assert [segment.name for segment in plan.segments] == [ "transfer", "approach", @@ -1007,7 +1094,7 @@ def fail_second_receiving_arm( return success, qpos generator.robot.compute_ik.side_effect = fail_second_receiving_arm - semantics = _semantics() + semantics = _semantics(entity_id="handover_object") task = TaskState( batch_size=NUM_ENVS, device="cpu", @@ -1039,7 +1126,8 @@ def fail_second_receiving_arm( motion_policy=MotionPolicy(sample_count=30), ) - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, False] @@ -1051,6 +1139,38 @@ def fail_second_receiving_arm( received = projected.get_held_object("right_arm") assert received is not None assert received.env_mask.tolist() == [True, False] + semantics.entity.get_local_pose.assert_not_called() + + +def test_handover_rejects_goal_for_a_different_held_object() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + HandOver( + default_options=HandOverOptions( + middle_object_pose=torch.eye(4), + final_object_pose=torch.eye(4), + ) + ), + ) + held_semantics = _semantics(entity_id="held_object") + goal_semantics = _semantics(entity_id="other_object") + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"left_arm": _held(held_semantics)}, + ) + invocation = ActionInvocation( + skill_id="hand_over", + goal=GraspGoal(semantics=goal_semantics), + binding=_dual_binding("source", "destination"), + ) + + with pytest.raises(ValueError, match="must identify the object held"): + _plan_action(action, invocation, _dual_context(task)) + + held_semantics.entity.get_local_pose.assert_not_called() + goal_semantics.entity.get_local_pose.assert_not_called() def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None: @@ -1067,8 +1187,14 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ) affordance = AntipodalAffordance() _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) semantics = ObjectSemantics( - affordance=affordance, geometry={}, label="coordinated-object" + affordance=affordance, + geometry={}, + label="coordinated-object", + entity=entity, + entity_id="coordinated_object", ) invocation = ActionInvocation( skill_id="coordinated_pickment", @@ -1082,11 +1208,14 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ) context = _dual_context() - plan = _plan_action(action, invocation, context) + request = action.resolve_request(invocation) + plan = action.plan(request, context) projected = plan.expected_effects.apply(context.task, plan.plan_success) assert plan.plan_success.tolist() == [True, True] assert plan.trajectory.positions.shape == (NUM_ENVS, 30, DUAL_ROBOT_DOF) + assert plan.scene_dependencies == () + request.goal.semantics.entity.get_local_pose.assert_not_called() assert projected.get_held_object("left_arm") is None assert projected.get_held_object("right_arm") is None assert isinstance( @@ -1102,6 +1231,129 @@ def test_coordinated_pick_returns_full_dof_plan_and_projected_relation() -> None ] +def test_coordinated_pick_implicit_initial_pose_uses_scene_snapshot() -> None: + generator = _dual_motion_generator() + action = _bind_action( + generator, + CoordinatedPickment( + default_options=CoordinatedPickmentOptions( + hand_interp_steps=4, + hold_steps=2, + object_motion_keyframes=3, + ), + ), + ) + affordance = AntipodalAffordance() + _stub_dual_arm_grasp_poses(affordance) + entity = Mock() + entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=affordance, + geometry={}, + label="snapshot-coordinated-object", + entity=entity, + entity_id="target", + ) + object_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + object_pose[:, :3, :3] = torch.tensor( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + object_pose[:, 1, 3] = torch.tensor([0.2, 0.4]) + invocation = ActionInvocation( + skill_id="coordinated_pickment", + goal=CoordinatedPickGoal( + semantics=semantics, + object_target_pose=object_pose, + ), + binding=_dual_binding("left", "right"), + motion_policy=MotionPolicy(sample_count=30), + ) + context = _dual_context(scene=_target_scene(object_pose, timestamp=0.0, version=0)) + + request = action.resolve_request(invocation) + plan = action.plan(request, context) + projected = plan.expected_effects.apply(context.task, plan.plan_success) + + resolved_affordance = request.goal.semantics.affordance + sampled_pose = resolved_affordance.get_dual_arm_valid_grasp_poses.call_args.kwargs[ + "obj_poses" + ] + assert torch.equal(sampled_pose, object_pose) + assert plan.scene_dependencies == ("target",) + request.goal.semantics.entity.get_local_pose.assert_not_called() + held = projected.get_coordinated_held_object("left_arm", "right_arm") + assert held is not None + assert torch.allclose(held.left_object_to_eef, pose_inv(object_pose)) + assert torch.allclose(held.right_object_to_eef, pose_inv(object_pose)) + + +def test_assemble_place_uses_explicit_base_snapshot() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.full((NUM_ENVS, 4, 4), 9.0) + relative_pose = torch.eye(4) + relative_pose[2, 3] = 0.05 + affordance = AssembleAffordance( + base_object_entity=base_entity, + assemble_to_base_pose=relative_pose, + ) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held(_semantics(entity_id="assemble_object"))}, + ) + base_pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + base_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) + context = _context( + task, + scene=SceneSnapshot( + timestamp=0.0, + version=0, + entities={"base": EntityState(base_pose)}, + ), + ) + + request = action.resolve_request( + _invocation( + "place", + AssembleGoal( + affordance=affordance, + base_pose=SceneEntityPose("base"), + ), + ) + ) + plan = action.plan(request, context) + + assert plan.plan_success.all() + assert plan.scene_dependencies == ("base",) + request.goal.affordance.base_object_entity.get_local_pose.assert_not_called() + + +def test_assemble_place_legacy_base_entity_warns() -> None: + generator = _motion_generator() + action = _bind_action(generator, Place()) + base_entity = Mock() + base_entity.get_local_pose.return_value = torch.eye(4) + affordance = AssembleAffordance(base_object_entity=base_entity) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects={"arm": _held()}, + ) + + request = action.resolve_request( + _invocation("place", AssembleGoal(affordance=affordance)) + ) + with pytest.warns(DeprecationWarning, match="base_pose"): + plan = action.plan(request, _context(task)) + + assert plan.scene_dependencies == () + request.goal.affordance.base_object_entity.get_local_pose.assert_called_once_with( + to_matrix=True + ) + + def test_coordinated_pick_holds_only_environment_with_ik_failure() -> None: generator = _dual_motion_generator() original_compute_ik = generator.robot.compute_ik.side_effect diff --git a/tests/sim/atomic_actions/test_affordance.py b/tests/sim/atomic_actions/test_affordance.py index 4c05e0844..d85dbfe3e 100644 --- a/tests/sim/atomic_actions/test_affordance.py +++ b/tests/sim/atomic_actions/test_affordance.py @@ -18,9 +18,11 @@ from __future__ import annotations -import torch from unittest.mock import Mock +import pytest +import torch + from embodichain.lab.sim.atomic_actions.affordance import ( Affordance, AntipodalAffordance, @@ -193,3 +195,16 @@ def test_get_assemble_object_pose_broadcasts_batched_relative_pose(self): result = aff.get_assemble_object_pose(base_pose) assert result.shape == (n_envs, 4, 4) assert torch.allclose(result, torch.bmm(base_pose, rel)) + + def test_get_assemble_object_pose_rejects_relative_batch_mismatch(self): + aff = AssembleAffordance(assemble_to_base_pose=torch.eye(4).repeat(3, 1, 1)) + base_pose = torch.eye(4).repeat(2, 1, 1) + + with pytest.raises(ValueError, match="batch size must match"): + aff.get_assemble_object_pose(base_pose) + + def test_get_assemble_object_pose_rejects_invalid_base_shape(self): + aff = AssembleAffordance() + + with pytest.raises(ValueError, match="base_pose must have shape"): + aff.get_assemble_object_pose(torch.eye(4).repeat(2, 1, 1, 1)) diff --git a/tests/sim/atomic_actions/test_core.py b/tests/sim/atomic_actions/test_core.py index 7383cde33..2a4ae179c 100644 --- a/tests/sim/atomic_actions/test_core.py +++ b/tests/sim/atomic_actions/test_core.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import FrozenInstanceError +from unittest.mock import Mock import pytest import torch @@ -26,15 +27,22 @@ from embodichain.lab.sim.atomic_actions import ( ActionBinding, ActionInvocation, + ActionOptions, + ActionPlan, Affordance, + AtomicAction, + CoordinatedHeldObjectState, DynamicCollisionMode, EndEffectorPoseGoal, EntityState, HeldObjectState, MotionPolicy, ObjectSemantics, + PlannerDiagnostics, PlanningContext, RecoveryPolicy, + ResolvedActionBinding, + ResolvedActionRequest, RobotObservation, SceneEntityPose, SceneSnapshot, @@ -43,24 +51,53 @@ TimedTrajectory, ) from embodichain.lab.sim.atomic_actions.goals import ( + _resolve_object_pose, collect_scene_dependencies, resolve_pose_goal, ) -def _semantics(label: str = "object") -> ObjectSemantics: - return ObjectSemantics(affordance=Affordance(), geometry={}, label=label) +def _semantics( + label: str = "object", + *, + entity_id: str | None = None, +) -> ObjectSemantics: + return ObjectSemantics( + affordance=Affordance(), + geometry={}, + label=label, + entity_id=entity_id, + ) -def _held(batch_size: int = 2) -> HeldObjectState: +def _held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, +) -> HeldObjectState: pose = torch.eye(4).repeat(batch_size, 1, 1) return HeldObjectState( - semantics=_semantics(), + semantics=semantics or _semantics(), object_to_eef=pose, grasp_xpos=pose, ) +def _coordinated_held( + batch_size: int = 2, + *, + semantics: ObjectSemantics | None = None, +) -> CoordinatedHeldObjectState: + pose = torch.eye(4).repeat(batch_size, 1, 1) + return CoordinatedHeldObjectState( + semantics=semantics or _semantics(), + left_object_to_eef=pose, + right_object_to_eef=pose, + left_grasp_xpos=pose, + right_grasp_xpos=pose, + ) + + def _context(scene: SceneSnapshot | None = None) -> PlanningContext: qpos = torch.zeros(2, 4) return PlanningContext( @@ -71,6 +108,42 @@ def _context(scene: SceneSnapshot | None = None) -> PlanningContext: ) +class _DependencyAction(AtomicAction[EndEffectorPoseGoal, ActionOptions]): + """Minimal action proving that build_plan delegates dependencies to its hook.""" + + skill_id = "dependency_test" + GoalType = EndEffectorPoseGoal + OptionsType = ActionOptions + manipulator_roles = () + + @property + def device(self) -> torch.device: + return torch.device("cpu") + + def _uses_collision_world( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> bool: + del request, context + return False + + def _scene_dependencies( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + ) -> tuple[str, ...]: + dependencies = set(super()._scene_dependencies(request)) + dependencies.add("extra") + return tuple(sorted(dependencies)) + + def _plan( + self, + request: ResolvedActionRequest[EndEffectorPoseGoal, ActionOptions], + context: PlanningContext, + ) -> ActionPlan: + raise NotImplementedError + + def test_action_binding_is_role_based_and_immutable() -> None: binding = ActionBinding( manipulators={"primary": "left_arm"}, @@ -94,6 +167,23 @@ def test_invocation_rejects_values_without_goal_contract() -> None: ) +@pytest.mark.parametrize("entity_id", ["", " ", 7]) +def test_object_semantics_rejects_invalid_entity_id(entity_id: object) -> None: + with pytest.raises(ValueError, match="entity_id"): + ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity_id=entity_id, # type: ignore[arg-type] + ) + + +def test_object_semantics_identity_fields_are_frozen() -> None: + semantics = _semantics(entity_id="cube") + + with pytest.raises(FrozenInstanceError): + semantics.entity_id = "other" # type: ignore[misc] + + def test_motion_and_recovery_policy_validate_shared_parameters() -> None: policy = MotionPolicy(sample_count=24, control_dt=0.01) assert policy.sample_count == 24 @@ -164,6 +254,137 @@ def test_task_state_normalizes_held_relations_and_masks_updates() -> None: assert state.get_held_object("right_arm") is None +def test_state_delta_merges_distinct_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert previous_semantics is not candidate_semantics + assert held is not None and held.semantics is previous_semantics + + +def test_state_delta_replaces_semantics_when_all_rows_are_updated() -> None: + previous_semantics = _semantics(entity_id="cube") + candidate_semantics = _semantics(entity_id="cube") + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, True])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is candidate_semantics + + +def test_state_delta_rejects_partial_merge_of_different_entity_ids() -> None: + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=_semantics(entity_id="cube"))}, + ) + delta = StateDelta( + held_object_updates={ + "arm": _held(semantics=_semantics(entity_id="cup")), + } + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_does_not_match_explicit_id_to_legacy_uid() -> None: + shared_entity = Mock(uid="cube") + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + entity_id="cube", + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=shared_entity, + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + delta = StateDelta( + held_object_updates={"arm": _held(semantics=candidate_semantics)} + ) + + with pytest.raises(ValueError, match="different held-object semantics"): + delta.apply(state, torch.tensor([True, False])) + + +def test_state_delta_merges_legacy_semantics_with_same_uid() -> None: + previous_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + candidate_semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=Mock(uid="cube"), + ) + state = TaskState( + batch_size=2, + device="cpu", + held_objects={"arm": _held(semantics=previous_semantics)}, + ) + + updated = StateDelta( + held_object_updates={ + "arm": _held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_held_object("arm") + assert held is not None and held.semantics is previous_semantics + + +def test_state_delta_merges_coordinated_semantics_with_same_entity_id() -> None: + previous_semantics = _semantics(entity_id="tray") + candidate_semantics = _semantics(entity_id="tray") + key = ("left_arm", "right_arm") + state = TaskState( + batch_size=2, + device="cpu", + coordinated_held_objects={ + key: _coordinated_held(semantics=previous_semantics), + }, + ) + + updated = StateDelta( + coordinated_held_object_updates={ + key: _coordinated_held(semantics=candidate_semantics), + } + ).apply(state, torch.tensor([True, False])) + + held = updated.get_coordinated_held_object(*key) + assert previous_semantics is not candidate_semantics + assert held is not None and held.semantics is previous_semantics + + def test_robot_observation_owns_input_tensors() -> None: qpos = torch.zeros(2, 4) observation = RobotObservation( @@ -214,6 +435,96 @@ def test_scene_entity_pose_enforces_confidence() -> None: ) +def test_object_pose_uses_explicit_scene_id_without_live_fallback() -> None: + scene_pose = torch.eye(4).repeat(2, 1, 1) + scene_pose[:, 0, 3] = torch.tensor([0.2, 0.4]) + entity = Mock() + entity.get_local_pose.return_value = torch.full((2, 4, 4), 9.0) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="cup", + ) + context = _context( + SceneSnapshot( + timestamp=1.0, + version=1, + entities={"cup": EntityState(scene_pose)}, + ) + ) + + resolved = _resolve_object_pose(semantics, context) + + assert torch.equal(resolved, scene_pose) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_missing_explicit_scene_id_does_not_fall_back() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4).repeat(2, 1, 1) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + entity_id="missing", + ) + + with pytest.raises(KeyError, match="unknown scene entity"): + _resolve_object_pose(semantics, _context()) + entity.get_local_pose.assert_not_called() + + +def test_object_pose_legacy_entity_warns_and_broadcasts() -> None: + entity = Mock() + entity.get_local_pose.return_value = torch.eye(4) + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + entity=entity, + ) + + with pytest.warns(DeprecationWarning, match="entity_id"): + resolved = _resolve_object_pose(semantics, _context()) + + assert resolved.shape == (2, 4, 4) + entity.get_local_pose.assert_called_once_with(to_matrix=True) + + +def test_dependency_collection_does_not_descend_object_semantics() -> None: + semantics = ObjectSemantics( + affordance=Affordance(), + geometry={}, + properties={"unrelated_pose": SceneEntityPose("hidden")}, + entity_id="object", + ) + + assert collect_scene_dependencies(semantics) == () + + +def test_build_plan_uses_action_scene_dependency_hook() -> None: + context = _context() + request = ResolvedActionRequest( + skill_id="dependency_test", + goal=EndEffectorPoseGoal(SceneEntityPose("tracked")), + binding=ResolvedActionBinding(), + motion_policy=MotionPolicy(), + recovery_policy=RecoveryPolicy(), + skill_options=ActionOptions(), + ) + action = _DependencyAction() + + plan = action.build_plan( + request, + context, + success=True, + trajectory=context.robot.qpos.unsqueeze(1), + diagnostics=PlannerDiagnostics(backend="test"), + ) + + assert plan.scene_dependencies == ("extra", "tracked") + + def test_scene_snapshot_expands_global_collision_world_revision() -> None: pose = torch.eye(4).repeat(2, 1, 1) snapshot = SceneSnapshot( diff --git a/tests/sim/atomic_actions/test_trajectory_ops.py b/tests/sim/atomic_actions/test_trajectory_ops.py index e3dae34a6..41c9c4ed5 100644 --- a/tests/sim/atomic_actions/test_trajectory_ops.py +++ b/tests/sim/atomic_actions/test_trajectory_ops.py @@ -83,6 +83,21 @@ def test_non_binary_integer_success_is_rejected(self): name="IK success", ) + def test_cuda_device_requires_available_runtime(self, monkeypatch): + def unexpected_current_device(): + raise AssertionError("current_device must not be queried") + + monkeypatch.setattr(torch.cuda, "is_available", lambda: False) + monkeypatch.setattr(torch.cuda, "current_device", unexpected_current_device) + + with pytest.raises(ValueError, match="CUDA device requested"): + normalize_success_mask( + True, + n_envs=2, + device="cuda", + name="IK success", + ) + class TestResolvePoseTarget: def test_unbatched_pose_broadcasts(self): @@ -279,6 +294,11 @@ def test_raises_when_first_segment_too_small(self): with pytest.raises(ValueError): split_three_segments(6, 5) + def test_ratio_is_rounded_after_multiplication(self): + first, hand, third = split_three_segments(10, 2) + + assert (first, hand, third) == (5, 2, 3) + class TestTranslatePoseWorld: def test_offset_adds_to_translation(self): diff --git a/tests/sim/planners/test_curobo_planner.py b/tests/sim/planners/test_curobo_planner.py index f62563d2c..ba86c7d4a 100644 --- a/tests/sim/planners/test_curobo_planner.py +++ b/tests/sim/planners/test_curobo_planner.py @@ -218,6 +218,44 @@ def test_curobo_world_cfg_uses_v2_safe_default_collision_cache(): assert cfg.obstacle_representation == "sphere" +def test_curobo_world_cfg_accepts_registered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + cfg = CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known"], + ) + + assert cfg.dynamic_obstacle_names == ["known"] + + +def test_curobo_world_cfg_rejects_unregistered_dynamic_obstacle(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="not present in rigid_objects"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["unknown"], + ) + + +def test_curobo_world_cfg_rejects_duplicate_dynamic_obstacle_names(): + obstacle = type("NamedObstacle", (), {"uid": "known"})() + + with pytest.raises(ValueError, match="unique non-empty"): + CuroboWorldCfg( + rigid_objects=[obstacle], + dynamic_obstacle_names=["known", "known"], + ) + + +def test_curobo_world_cfg_rejects_duplicate_rigid_object_names(): + obstacle_type = type("NamedObstacle", (), {"uid": "duplicate"}) + + with pytest.raises(ValueError, match="unique obstacle names"): + CuroboWorldCfg(rigid_objects=[obstacle_type(), obstacle_type()]) + + def test_curobo_collision_world_binding_merges_owned_obstacle_poses(): planner = object.__new__(CuroboPlanner) configured_pose = torch.eye(4).unsqueeze(0)