Add axis_align, pour and hand over

docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.primitives.rst

@@ -23,8 +23,12 @@ full-robot timed trajectory and uncommitted expected effects.
       MoveJointsOptions
       PickUp
       PickUpOptions
+      AxisAlign
+      AxisAlignOptions
       MoveHeldObject
       MoveHeldObjectOptions
+      Pour
+      PourOptions
       Place
       PlaceOptions
       Press
@@ -47,7 +51,9 @@ full-robot timed trajectory and uncommitted expected effects.
       EndEffectorPoseGoal
       JointPositionGoal
       GraspGoal
+      AxisAlignGoal
       HeldObjectPoseGoal
+      PourGoal
       PlaceGoal
       AssembleGoal
       PressGoal
@@ -82,6 +88,14 @@ PickUp
    :show-inheritance:
    :exclude-members: __init__, copy, replace, to_dict
 
+AxisAlign
+---------
+
+.. automodule:: embodichain.lab.sim.atomic_actions.primitives.axis_align
+   :members:
+   :show-inheritance:
+   :exclude-members: __init__, copy, replace, to_dict
+
 MoveHeldObject
 --------------
 
@@ -90,6 +104,14 @@ MoveHeldObject
    :show-inheritance:
    :exclude-members: __init__, copy, replace, to_dict
 
+Pour
+----
+
+.. automodule:: embodichain.lab.sim.atomic_actions.primitives.pour
+   :members:
+   :show-inheritance:
+   :exclude-members: __init__, copy, replace, to_dict
+
 Place
 -----
 

docs/source/api_reference/embodichain/embodichain.lab.sim.atomic_actions.rst

@@ -76,7 +76,12 @@ embodichain.lab.sim.atomic_actions
       EndEffectorPoseGoal
       JointPositionGoal
       GraspGoal
+      AxisAlignGoal
+      AxisAlignOptions
+      AxisAlignAffordance
       HeldObjectPoseGoal
+      PourGoal
+      PourOptions
       PlaceGoal
       AssembleGoal
       PressGoal
@@ -93,7 +98,9 @@ embodichain.lab.sim.atomic_actions
       MoveEndEffector
       MoveJoints
       PickUp
+      AxisAlign
       MoveHeldObject
+      Pour
       Place
       Press
       Slide

docs/source/overview/sim/atomic_actions/builtin_actions.md

@@ -5,7 +5,7 @@
 ```{currentmodule} embodichain.lab.sim.atomic_actions
 ```
 
-EmbodiChain ships eleven built-in action implementations with stable skill IDs;
+EmbodiChain ships thirteen built-in action implementations with stable skill IDs;
 `AtomicActionEngine` creates and registers a fresh instance of every built-in by
 default. Applications select them by stable skill ID rather than registering
 routine instances themselves.
@@ -68,6 +68,13 @@ The animations below are the focused simulator demos under
 <img src="../../../_static/atomic_actions/pickup.gif" alt="PickUp demo" width="480" style="max-width: 100%;" />
 :::
 
+:::{grid-item-card} `AxisAlign`
+:link: builtin-axis-align
+:link-type: ref
+
+`axis_align` · grasp, lift, align an object-local axis, and release
+:::
+
 :::{grid-item-card} `MoveHeldObject`
 :link: builtin-move-held-object
 :link-type: ref
@@ -77,6 +84,13 @@ The animations below are the focused simulator demos under
 <img src="../../../_static/atomic_actions/move_held_object.gif" alt="MoveHeldObject demo" width="480" style="max-width: 100%;" />
 :::
 
+:::{grid-item-card} `Pour`
+:link: builtin-pour
+:link-type: ref
+
+`pour` · rotate an already-held object about its local internal axis
+:::
+
 :::{grid-item-card} `Place`
 :link: builtin-place
 :link-type: ref
@@ -160,6 +174,7 @@ The animations below are the focused simulator demos under
 | `move_joints` | `JointPositionGoal` | manipulator `primary` | named target only: command matching `target` | none | none |
 | `pick_up` | `GraspGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | semantic object/entity | attach object to `primary` manipulator |
 | `move_held_object` | `HeldObjectPoseGoal` | manipulator + end effector `primary` | primary: `grasp` | object held by `primary` | preserve attachment |
+| `pour` | `PourGoal` | manipulator + end effector `primary` | primary: `grasp` | exclusively held object with `AxisAlignAffordance` | preserve attachment; open-loop rotate and return |
 | `place` | `PlaceGoal`, `AssembleGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `AssembleGoal` requires an object held by `primary`; ordinary `PlaceGoal` has no planner-enforced attachment precondition | detach object |
 | `press` | `PressGoal` | manipulator + end effector `primary` | primary: `grasp` | `PressAffordance` + target pose | open-loop motion; application verifies contact/actuation |
 | `slide` | `SlideGoal` | manipulator + end effector `primary` | primary: `open`, `grasp` | `SlideAffordance` + link pose | open-loop motion; application verifies joint travel/grasp |
@@ -224,6 +239,8 @@ entity as a recovery dependency.
 | `CoordinatedPlacementGoal` placing/support poses | yes | yes |
 | `PickUp.grasp_xpos` | yes | yes |
 | `PickUp` `ObjectSemantics.entity_id` grounding | implicit snapshot reference | yes; always consumed for the object pose |
+| `AxisAlign.grasp_xpos` | yes | yes |
+| `AxisAlign` `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 |
@@ -399,6 +416,49 @@ deprecated entity-only fallback. For canonical snapshot grounding and moving
 target recovery, see
 `scripts/tutorials/atomic_action/moving_target_recovery.py`.
 
+(builtin-axis-align)=
+
+## `AxisAlign`
+
+Executes **approach -> reach -> close -> lift -> align -> lower -> open** while
+grouping arm motion into two planner calls: the open-gripper `approach` phase
+contains the pre-grasp and grasp targets, and the closed-gripper `manipulate`
+phase contains lift, alignment, and lowering targets. The `close` and `open`
+segments are local hand interpolation and do not call the motion generator.
+Only the final aligned pose is sent to the planner; the alignment sample budget
+controls trajectory resolution without expanding the rotation into one CuRobo
+`plan_pose` call per intermediate orientation.
+The object's `AxisAlignAffordance.internal_axis` is expressed in the
+object-local frame, while `AxisAlignOptions.target_axis` is expressed in the
+world frame. The final alignment target applies the shortest rotation about
+the lifted object's origin so that
+`aligned_object_rotation @ internal_axis == normalized_target_axis`, then
+derives every end-effector keyframe through the fixed grasp transform.
+
+| Contract | Value |
+|---|---|
+| Skill ID | `axis_align` |
+| Goal | `AxisAlignGoal(semantics=..., grasp_xpos=None)` |
+| Binding | manipulator + end effector role `primary` |
+| Precondition | an `AxisAlignAffordance`; the object pose resolves from `ObjectSemantics.entity_id` or the deprecated live entity fallback |
+| Motion | approach, grasp, lift, rotate in place, lower, release |
+| Effect | explicitly open-loop; no final object-pose success is claimed |
+
+An explicit `grasp_xpos` accepts the same pose forms as `GraspGoal`; omitting it
+prefers valid antipodal grasps whose TCP y-axis is perpendicular to the object
+rotation axis, using grasp cost as the tie-breaker. When a currently horizontal
+object axis is aligned to world-up, the initial grasp orientation is pre-rotated
+45 degrees opposite the alignment rotation. This reduces the arm's table-side
+sweep during upright manipulation. `AxisAlignOptions` extends `PickUpOptions`
+with `target_axis` and `lower_distance`. Shared and per-environment target axes
+use shapes `(3,)` and `(B, 3)` respectively. Zero or non-finite axes are
+rejected, and exactly opposite axes use a deterministic 180-degree rotation
+rather than an unstable cross-product direction.
+
+**Example:** `scripts/tutorials/atomic_action/axis_align.py` provides
+`--alignment upright` (align object-local X to world Z) and
+`--alignment horizontal_align` (align object-local X to world Y).
+
 (builtin-move-held-object)=
 
 ## `MoveHeldObject`
@@ -429,6 +489,38 @@ same semantic object or live entity are marked unsuccessful and held in place.
 
 **Example:** `scripts/tutorials/atomic_action/move_held_object.py`
 
+(builtin-pour)=
+
+## `Pour`
+
+Rotates the object already held by the bound `primary` manipulator. `PourGoal`
+contains no object pose because the action consumes the verified
+`HeldObjectState` created by `PickUp`. `PourOptions` contains only the signed
+`rotate_angle` in radians.
+
+The held object's semantics must use `AxisAlignAffordance`. The action obtains
+the current EEF pose from FK at the observed starting arm qpos, reconstructs
+the current object pose using `eef_pose @ inverse(object_to_eef)`, transforms
+the affordance's object-local `internal_axis` into world space, and applies the
+requested rotation while keeping the object origin fixed. The resulting EEF
+target is `target_object_pose @ object_to_eef`. A second target then returns to
+the EEF pose observed by FK, thereby reversing the rotation by the same angle.
+Both targets are submitted in one motion-generation call so a collision-aware
+planner can chain the outbound and return legs.
+
+| Contract | Value |
+|---|---|
+| Skill ID | `pour` |
+| Goal | `PourGoal()` |
+| Options | `PourOptions(rotate_angle=...)` |
+| Binding | manipulator + end effector role `primary` |
+| Precondition | an exclusive `HeldObjectState` whose semantics use `AxisAlignAffordance` |
+| Motion | rotate by `rotate_angle`, then reverse by the same angle, with the hand held at `grasp` |
+| Effect | none; the existing attachment is preserved |
+
+**Example:** `scripts/tutorials/atomic_action/pour.py` compiles a horizontal
+`PickUp` followed by `Pour`.
+
 (builtin-place)=
 
 ## `Place`

docs/source/tutorial/atomic_actions.rst

@@ -107,6 +107,7 @@ Focused examples live under ``scripts/tutorials/atomic_action``:
 * ``control_dt.py``
 * ``pickup.py``
 * ``move_held_object.py``
+* ``pour.py``
 * ``place.py``
 * ``assemble.py``
 * ``press.py``
@@ -127,6 +128,7 @@ video under ``outputs/videos``:
    python scripts/tutorials/atomic_action/move_end_effector.py --headless --auto_play --device cpu
    python scripts/tutorials/atomic_action/control_dt.py --headless --auto_play --device cpu
    python scripts/tutorials/atomic_action/pickup.py --headless --auto_play --device cpu
+   python scripts/tutorials/atomic_action/pour.py --headless --auto_play --device cpu
    python scripts/tutorials/atomic_action/assemble.py --headless --auto_play --device cpu
    python scripts/tutorials/atomic_action/hand_over.py --headless --auto_play --device cpu
 

embodichain/lab/sim/atomic_actions/__init__.py

@@ -32,6 +32,7 @@
     Affordance,
     AntipodalAffordance,
     AssembleAffordance,
+    AxisAlignAffordance,
     InteractionPoints,
     PressAffordance,
     SlideAffordance,
@@ -85,6 +86,9 @@
 from .runtime import ActionPlanningServices
 from .primitives import (
     AssembleGoal,
+    AxisAlign,
+    AxisAlignGoal,
+    AxisAlignOptions,
     BUILTIN_ACTION_TYPES,
     CoordinatedPickGoal,
     CoordinatedPickment,
@@ -109,6 +113,9 @@
     Place,
     PlaceGoal,
     PlaceOptions,
+    Pour,
+    PourGoal,
+    PourOptions,
     Press,
     PressGoal,
     PressOptions,
@@ -163,6 +170,10 @@
     "AntipodalAffordance",
     "AssembleAffordance",
     "AssembleGoal",
+    "AxisAlign",
+    "AxisAlignAffordance",
+    "AxisAlignGoal",
+    "AxisAlignOptions",
     "AtomicAction",
     "AtomicActionEngine",
     "BUILTIN_ACTION_TYPES",
@@ -228,6 +239,9 @@
     "Place",
     "PlaceGoal",
     "PlaceOptions",
+    "Pour",
+    "PourGoal",
+    "PourOptions",
     "PlannerDiagnostics",
     "PlanningContext",
     "PoseGoalValue",

embodichain/lab/sim/atomic_actions/affordance.py

@@ -233,6 +233,29 @@ def get_best_grasp_poses(
         return is_success_t, grasp_xpos, open_length_t
 
 
+@dataclass
+class AxisAlignAffordance(AntipodalAffordance):
+    """Antipodal grasp affordance with an object-local alignment axis."""
+
+    internal_axis: torch.Tensor = field(
+        default_factory=lambda: torch.tensor([0.0, 0.0, 1.0])
+    )
+    """Axis expressed in the target object's local frame."""
+
+    def __post_init__(self) -> None:
+        if (
+            not isinstance(self.internal_axis, torch.Tensor)
+            or self.internal_axis.shape != (3,)
+            or not torch.isfinite(self.internal_axis).all()
+        ):
+            raise ValueError(
+                "AxisAlignAffordance.internal_axis must be a finite (3,) tensor."
+            )
+        if torch.linalg.vector_norm(self.internal_axis) <= 1.0e-6:
+            raise ValueError("AxisAlignAffordance.internal_axis must be non-zero.")
+        self.internal_axis = self.internal_axis.clone()
+
+
 @dataclass
 class TwistAffordance(Affordance):
     """Target-local grasp point and rotation-axis geometry for twisting."""
@@ -617,6 +640,7 @@ def get_assemble_object_pose(self, base_pose: torch.Tensor) -> torch.Tensor:
 __all__ = [
     "Affordance",
     "AntipodalAffordance",
+    "AxisAlignAffordance",
     "SlideAffordance",
     "PressAffordance",
     "TwistAffordance",

embodichain/lab/sim/atomic_actions/primitives/__init__.py

@@ -19,6 +19,7 @@
 from __future__ import annotations
 
 from ..core import AtomicAction
+from .axis_align import AxisAlign, AxisAlignGoal, AxisAlignOptions
 from .coordinated_pickment import (
     CoordinatedPickGoal,
     CoordinatedPickment,
@@ -44,6 +45,7 @@
 from .pick_up import GraspGoal, PickUp, PickUpOptions
 from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions
 from .press import Press, PressGoal, PressOptions
+from .pour import Pour, PourGoal, PourOptions
 from .slide import (
     Slide,
     SlideGoal,
@@ -55,8 +57,10 @@
     MoveEndEffector,
     MoveJoints,
     PickUp,
+    AxisAlign,
     MoveHeldObject,
     Place,
+    Pour,
     Press,
     Slide,
     Twist,
@@ -68,6 +72,9 @@
 
 __all__ = [
     "AssembleGoal",
+    "AxisAlign",
+    "AxisAlignGoal",
+    "AxisAlignOptions",
     "BUILTIN_ACTION_TYPES",
     "CoordinatedPickGoal",
     "CoordinatedPickment",
@@ -92,6 +99,9 @@
     "Place",
     "PlaceGoal",
     "PlaceOptions",
+    "Pour",
+    "PourGoal",
+    "PourOptions",
     "Press",
     "PressGoal",
     "PressOptions",