diff --git a/docs/design/declarative_expert_program_plan.md b/docs/design/declarative_expert_program_plan.md index e8226f62b..419d67bd4 100644 --- a/docs/design/declarative_expert_program_plan.md +++ b/docs/design/declarative_expert_program_plan.md @@ -1,13 +1,15 @@ # Declarative Expert Programs and Unified Semantic Skill Runtime -- Status: implementation in progress; Phase 0 and PR1 complete, PR2A and PR2B - implemented on stacked feature branches -- Baseline: `main@e445133c79c8b32019dab1c844b799b43a1658d6` -- Last updated: 2026-08-10 +- Status: implementation in progress; Phase 0 and Phase 1 are complete on + `main`, and Phase 2 is next +- Baseline: `main@f4ffb6608f41ea1eaee3020714412e31549dbda7` +- Last updated: 2026-08-18 - 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) + [#475](https://github.com/DexForce/EmbodiChain/pull/475), + [#517](https://github.com/DexForce/EmbodiChain/pull/517), + [#487](https://github.com/DexForce/EmbodiChain/pull/487) ## 1. Executive summary @@ -91,12 +93,11 @@ sessions, or verifiers. ## 4. Baseline on current `main` -This plan is updated against committed `main@e445133c` after PR #475. The -implementation series is stacked from that baseline: PR1 is complete on -`refactor/atomic-actions-phase0`, PR2A is implemented by -`feat/atomic-action-pr2a-scene-registry`, and PR2B is implemented by -`feat/atomic-action-pr2b-robot-skill-profile`. These status statements do not -imply that the stacked changes have landed on `main`. +This plan is updated against committed `main@f4ffb660`. PR #517 simplified the +atomic-action core, and PR #487 landed the complete Phase 1 foundation. The +scene registry and robot skill profile APIs are available, but official task +environments have not adopted them yet; that rollout starts only after the +semantic compiler and runtime exist. | Capability | Current main | Design consequence | |---|---|---| @@ -107,6 +108,8 @@ imply that the stacked changes have landed on `main`. | 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()`. | +| Authoritative scene registry (#487) | Foundation available; official environments not migrated | Reuse it from the semantic compiler and opt in task scenes explicitly. | +| Declarative robot skill profiles (#487) | Foundation available; official profiles not yet installed | Bind reusable embodiment profiles through the semantic integration layer. | | 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. | @@ -119,27 +122,16 @@ 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 `main` are: +Phase 1 closes the core identity, registry-backed collision integration, and +embodiment-owned capability/profile prerequisites. The remaining #474 work is +adoption and semantic orchestration: -- 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; -- robot capability declarations, resource selection, semantic commands, and - stable policies do not yet have an embodiment-owned source of truth; -- 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. - -PR1 closes the snapshot-grounding and stable-identity bridge. PR2A closes the -first and third gaps for registry-backed integrations by introducing one -authoritative registration boundary, a registry-derived scene provider, and -construction-time collision-world validation. PR2B closes the robot-profile -gap on its stacked branch with generic resources, deterministic binding, -profile-owned commands, and named policy presets. The semantic facade remains -later-phase work. +- official environments do not yet provide authoritative registry population; +- official robot configurations do not yet install reusable skill profiles; +- named presets are not yet selected by a semantic facade/runtime; and +- effect monitoring and the configuration/demo path remain later-phase work. One #474 finding has changed since its review branch: the ambiguous `collision_check` switch has been replaced by `DynamicCollisionMode.OFF`, @@ -912,8 +904,8 @@ Landed on `main` through #475: 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`; this remains the foundation for the -completed PR1 bridge. That bridge adds neither a legacy `plan()` adapter nor a +Exit criteria were met by PR #475 and remain the foundation for the completed +Phase 1 bridge. That bridge adds neither a legacy `plan()` adapter nor a pre-registry duplicate of the integration-level obstacle validator. ### PR1: core snapshot and identity bridge (complete) @@ -956,18 +948,18 @@ 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 are met on `refactor/atomic-actions-phase0`: 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. +Exit criteria are met on `main` through PR #487: 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. ### Phase 1: unified integration data Phase 1 is implemented as two focused follow-up PRs that join before the semantic facade/compiler work. -#### PR2A: SceneRegistry (implemented on the feature branch) +#### PR2A: SceneRegistry (landed in PR #487) Deliverables: @@ -1006,7 +998,7 @@ dynamic registry/provider/planner subsets are validated before execution, the collision-world batch mode agrees, and cuRobo uses canonical mapping keys end to end as logical source IDs (and as physical keys for cuboid/mesh worlds). -#### PR2B: RobotSkillProfile (implemented on the feature branch) +#### PR2B: RobotSkillProfile (landed in PR #487) Deliverables: @@ -1032,9 +1024,9 @@ new endpoint kind still needs one shared adapter and a compatible shared atomic skill before the current core can execute it; adding tasks that reuse that capability then remains configuration-only. -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. +PR2A and PR2B landed together through PR #487. That foundation does not migrate +official tasks; the repeated-cube vertical slice opts in only after the +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 diff --git a/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py new file mode 100644 index 000000000..d92255e9a --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/_binding_contracts.py @@ -0,0 +1,77 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Shared binding-contract declarations for built-in manipulation skills.""" + +from __future__ import annotations + +from collections.abc import Mapping + +from ..control import ControlCommand +from ..requirements import ( + ActionBindingRoute, + DisjointSlotEndpoints, + GRASP_CAPABILITY, + SkillEndpointRequirement, + SkillResourceSlot, +) + + +def make_motion_slot( + role: str, + *, + capabilities: frozenset[str], +) -> SkillResourceSlot: + """Build one current-core manipulator slot.""" + return SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=capabilities, + route=ActionBindingRoute("manipulator", role), + ), + ), + ) + + +def make_manipulation_slot( + role: str, + *, + motion_capabilities: frozenset[str], + grasp_commands: Mapping[str, type[ControlCommand]], +) -> SkillResourceSlot: + """Build one disjoint motion-and-grasp participant slot.""" + return SkillResourceSlot( + slot_id=role, + endpoints=( + SkillEndpointRequirement( + endpoint_id="motion", + capabilities=motion_capabilities, + route=ActionBindingRoute("manipulator", role), + ), + SkillEndpointRequirement( + endpoint_id="grasp", + capabilities=frozenset({GRASP_CAPABILITY}), + required_commands=grasp_commands, + route=ActionBindingRoute("end_effector", role), + ), + ), + constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + ) + + +__all__ = ["make_manipulation_slot", "make_motion_slot"] diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py index 84d1d7f5b..7829f72ec 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_pickment.py @@ -41,14 +41,9 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, DisjointResourceSlots, - DisjointSlotEndpoints, - GRASP_CAPABILITY, INVERSE_KINEMATICS_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import interpolate_joint_trajectory, translate_pose_world @@ -57,6 +52,7 @@ repeat_qpos, resolve_batched_pose, ) +from ._binding_contracts import make_manipulation_slot @dataclass(frozen=True, slots=True, eq=False) @@ -344,25 +340,13 @@ class CoordinatedPickment( end_effector_roles: ClassVar[tuple[str, ...]] = ("left", "right") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=tuple( - SkillResourceSlot( - slot_id=role, - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), - route=ActionBindingRoute("manipulator", role), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", role), - ), - ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + make_manipulation_slot( + role, + motion_capabilities=frozenset({INVERSE_KINEMATICS_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ) for role in ("left", "right") ), diff --git a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py index 49e606b4d..9db4563f7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py +++ b/embodichain/lab/sim/atomic_actions/primitives/coordinated_placement.py @@ -33,14 +33,9 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, - DisjointSlotEndpoints, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( @@ -54,6 +49,7 @@ resolve_batched_pose, resolve_object_target, ) +from ._binding_contracts import make_manipulation_slot @dataclass(frozen=True, slots=True, eq=False) @@ -146,42 +142,18 @@ class CoordinatedPlacement( end_effector_roles: ClassVar[tuple[str, ...]] = ("placing", "support") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="placing", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "placing"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "placing"), - ), - ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + make_manipulation_slot( + "placing", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), - SkillResourceSlot( - slot_id="support", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "support"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "support"), - ), - ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + make_manipulation_slot( + "support", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, ), ), constraints=(DisjointResourceSlots(("placing", "support")),), diff --git a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py index de1379076..04b635c04 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_actions/primitives/hand_over.py @@ -33,15 +33,10 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan, normalize_success_mask from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, DisjointResourceSlots, - DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( @@ -54,6 +49,7 @@ repeat_qpos, resolve_batched_pose, ) +from ._binding_contracts import make_manipulation_slot from .pick_up import GraspGoal @@ -154,50 +150,26 @@ class HandOver(AtomicAction[GraspGoal, HandOverOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("source", "destination") binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="source", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "source"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "source"), - ), + make_manipulation_slot( + "source", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), - SkillResourceSlot( - slot_id="destination", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "destination"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "destination"), - ), - ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + make_manipulation_slot( + "destination", + motion_capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), ), constraints=(DisjointResourceSlots(("source", "destination")),), diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py index 2c28d8041..78a35fcde 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_end_effector.py @@ -27,19 +27,14 @@ from ..goals import PoseGoalValue, resolve_pose_goal, validate_pose_goal from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan -from ..requirements import ( - ActionBindingRoute, - CARTESIAN_POSE_CAPABILITY, - SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, -) +from ..requirements import CARTESIAN_POSE_CAPABILITY, SkillBindingContract from ..state import PlanningContext from ..trajectory_ops import ( build_pose_plan_states, resolve_pose_target, to_full_robot_trajectory, ) +from ._binding_contracts import make_motion_slot @dataclass(frozen=True, slots=True, eq=False) @@ -65,15 +60,9 @@ class MoveEndEffector(AtomicAction[EndEffectorPoseGoal, MoveEndEffectorOptions]) GoalType: ClassVar[type] = EndEffectorPoseGoal binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), - ), - ), + make_motion_slot( + "primary", + capabilities=frozenset({CARTESIAN_POSE_CAPABILITY}), ), ), ) 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 3b28b6fdf..0f8d014e0 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_held_object.py @@ -36,17 +36,13 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import PlanningContext from ..trajectory_ops import build_pose_plan_states +from ._binding_contracts import make_manipulation_slot @dataclass(frozen=True, slots=True, eq=False) @@ -98,27 +94,15 @@ class MoveHeldObject(AtomicAction[HeldObjectPoseGoal, MoveHeldObjectOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), - ), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, ), ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py index 4cea2ab34..60d9383da 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_actions/primitives/move_joints.py @@ -26,19 +26,14 @@ from ..core import AtomicAction from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan -from ..requirements import ( - ActionBindingRoute, - JOINT_POSITION_CAPABILITY, - SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, -) +from ..requirements import JOINT_POSITION_CAPABILITY, SkillBindingContract from ..state import PlanningContext from ..trajectory_ops import ( build_joint_plan_states, resolve_joint_target, to_full_robot_trajectory, ) +from ._binding_contracts import make_motion_slot @dataclass(frozen=True, slots=True, eq=False) @@ -82,15 +77,9 @@ class MoveJoints(AtomicAction[JointPositionGoal, MoveJointsOptions]): agent_visible: ClassVar[bool] = False binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset({JOINT_POSITION_CAPABILITY}), - route=ActionBindingRoute("manipulator", "primary"), - ), - ), + make_motion_slot( + "primary", + capabilities=frozenset({JOINT_POSITION_CAPABILITY}), ), ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py index d2d71bf43..dea984d1d 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_actions/primitives/pick_up.py @@ -49,15 +49,10 @@ from ..plans import ActionPlan, normalize_success_mask from ..policies import MotionPolicy from ..requirements import ( - ActionBindingRoute, BATCH_INVERSE_KINEMATICS_CAPABILITY, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import HeldObjectState, PlanningContext from ..trajectory_ops import ( @@ -67,6 +62,7 @@ split_three_segments, translate_pose_world, ) +from ._binding_contracts import make_manipulation_slot @dataclass(frozen=True, slots=True, eq=False) @@ -166,31 +162,19 @@ class PickUp(AtomicAction[GraspGoal, PickUpOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - BATCH_INVERSE_KINEMATICS_CAPABILITY, - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "primary"), - ), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + BATCH_INVERSE_KINEMATICS_CAPABILITY, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/place.py b/embodichain/lab/sim/atomic_actions/primitives/place.py index 86ae1f599..6bee6fd38 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/place.py +++ b/embodichain/lab/sim/atomic_actions/primitives/place.py @@ -40,14 +40,9 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, FORWARD_KINEMATICS_CAPABILITY, - GRASP_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import PlanningContext from ..trajectory_ops import ( @@ -56,6 +51,7 @@ resolve_pose_target, split_three_segments, ) +from ._binding_contracts import make_manipulation_slot TcpSymmetry = Literal["none", "z_roll_180"] @@ -169,30 +165,18 @@ class Place(AtomicAction[PlaceGoal | AssembleGoal, PlaceOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - route=ActionBindingRoute("end_effector", "primary"), - ), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, ), ), ) diff --git a/embodichain/lab/sim/atomic_actions/primitives/press.py b/embodichain/lab/sim/atomic_actions/primitives/press.py index d9d3ff920..4310451f7 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/press.py +++ b/embodichain/lab/sim/atomic_actions/primitives/press.py @@ -30,14 +30,9 @@ from ..invocation import ActionOptions, ResolvedActionRequest from ..plans import ActionPlan from ..requirements import ( - ActionBindingRoute, CARTESIAN_POSE_CAPABILITY, - DisjointSlotEndpoints, - GRASP_CAPABILITY, JOINT_POSITION_CAPABILITY, SkillBindingContract, - SkillEndpointRequirement, - SkillResourceSlot, ) from ..state import PlanningContext from ..trajectory_ops import ( @@ -46,6 +41,7 @@ interpolate_hand_qpos, resolve_pose_target, ) +from ._binding_contracts import make_manipulation_slot @dataclass(frozen=True, slots=True, eq=False) @@ -81,27 +77,15 @@ class Press(AtomicAction[PressGoal, PressOptions]): end_effector_roles: ClassVar[tuple[str, ...]] = ("primary",) binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( - SkillResourceSlot( - slot_id="primary", - endpoints=( - SkillEndpointRequirement( - endpoint_id="motion", - capabilities=frozenset( - { - CARTESIAN_POSE_CAPABILITY, - JOINT_POSITION_CAPABILITY, - } - ), - route=ActionBindingRoute("manipulator", "primary"), - ), - SkillEndpointRequirement( - endpoint_id="grasp", - capabilities=frozenset({GRASP_CAPABILITY}), - required_commands={GRASP_COMMAND: JointPositionCommand}, - route=ActionBindingRoute("end_effector", "primary"), - ), + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + { + CARTESIAN_POSE_CAPABILITY, + JOINT_POSITION_CAPABILITY, + } ), - constraints=(DisjointSlotEndpoints(("motion", "grasp")),), + grasp_commands={GRASP_COMMAND: JointPositionCommand}, ), ), ) diff --git a/embodichain/lab/sim/skills/profiles.py b/embodichain/lab/sim/skills/profiles.py index 0e1d1a8c9..3a19510fb 100644 --- a/embodichain/lab/sim/skills/profiles.py +++ b/embodichain/lab/sim/skills/profiles.py @@ -1583,7 +1583,7 @@ def _assignments( resource for resource in self._resources.values() if (selected is None or resource.resource_id == selected) - and self._resource_matches(resource, slot) + and not self._rejection_reasons(resource, slot) ) if not candidates: return () @@ -1598,42 +1598,6 @@ def _assignments( assignments.append(assignment) return tuple(assignments) - def _resource_matches( - self, - resource: ResolvedRobotResource, - slot: SkillResourceSlot, - ) -> bool: - """Return whether one resource satisfies all slot-local endpoints.""" - matched_endpoints: dict[str, ResolvedResourceEndpoint] = {} - for requirement in slot.endpoints: - endpoint = resource.endpoints.get(requirement.endpoint_id) - if endpoint is None: - return False - if not requirement.capabilities.issubset(endpoint.capabilities): - return False - if ( - requirement.route is not None - and requirement.route.target not in endpoint.binding_values - ): - return False - for command_name, command_type in requirement.required_commands.items(): - command = endpoint.commands.get(command_name) - if not isinstance(command, command_type): - return False - matched_endpoints[requirement.endpoint_id] = endpoint - for constraint in slot.constraints: - if isinstance(constraint, DisjointSlotEndpoints): - endpoints = [ - matched_endpoints[endpoint_id] - for endpoint_id in constraint.endpoint_ids - ] - for index, left in enumerate(endpoints): - if any( - left.conflicts_with(right) for right in endpoints[index + 1 :] - ): - return False - return True - @staticmethod def _constraints_match( contract: SkillBindingContract,