remove lower xpos in axis align

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

@@ -73,7 +73,7 @@ The animations below are the focused simulator demos under
 :link: builtin-axis-align
 :link-type: ref
 
-`axis_align` · grasp, lift, align an object-local axis, and release
+`axis_align` · grasp, lift, and align an object-local axis
 
 <img src="../../../_static/atomic_actions/axis_align_horizontal.gif" alt="Axis align horizontal demo" width="480" style="max-width: 100%;" />
 <img src="../../../_static/atomic_actions/axis_align_upright.gif" alt="Axis align upright demo" width="480" style="max-width: 100%;" />
@@ -195,7 +195,7 @@ The animations below are the focused simulator demos under
 | `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | none | none | none |
 | `move_joints` | `JointPositionGoal` | `primary.motion` | named target only: command matching `target` on `primary.motion` | none | none |
 | `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object/entity | attach object to the `primary.motion` target |
-| `axis_align` | `AxisAlignGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | unheld object with `AxisAlignAffordance` | open-loop pick, align, lower, and release |
+| `axis_align` | `AxisAlignGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | unheld object with `AxisAlignAffordance` | open-loop pick and align while retaining the grasp |
 | `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held exclusively by the `primary.motion` target | preserve attachment |
 | `pour` | `PourGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | exclusively held object with `AxisAlignAffordance` | preserve attachment; open-loop rotate and return |
 | `push_object` | `PushObjectGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | free rigid object plus target support pose | open-loop planar push; application validates the measured landing pose |
@@ -463,11 +463,11 @@ target recovery, see
 
 ## `AxisAlign`
 
-Executes **approach -> reach -> close -> lift -> align -> lower -> open** while
+Executes **approach -> reach -> close -> lift -> align** 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.
+phase contains the lift and alignment targets. The `close` segment is local
+hand interpolation and does 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.
@@ -484,7 +484,7 @@ derives every end-effector keyframe through the fixed grasp transform.
 | 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 |
+| Motion | approach, grasp, lift, and rotate in place while retaining the grasp |
 | Effect | explicitly open-loop; no final object-pose success is claimed |
 
 An explicit `grasp_xpos` accepts the same pose forms as `GraspGoal`; omitting it
@@ -493,8 +493,8 @@ 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
+with `target_axis`. 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.
 

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

@@ -94,9 +94,6 @@ class AxisAlignOptions(PickUpOptions):
     target_axis: torch.Tensor = torch.tensor([0.0, 0.0, 1.0])
     """Desired world-frame axis, shape ``(3,)`` or ``(B, 3)``."""
 
-    lower_distance: float = 0.03
-    """World-Z distance (m) to lower the aligned object before release."""
-
     def __post_init__(self) -> None:
         PickUpOptions.__post_init__(self)
         if (
@@ -108,15 +105,11 @@ def __post_init__(self) -> None:
             raise ValueError("target_axis must be a finite (3,) or (B, 3) tensor.")
         if torch.any(torch.linalg.vector_norm(self.target_axis, dim=-1) <= 1.0e-6):
             raise ValueError("target_axis must be non-zero.")
-        if not math.isfinite(self.lower_distance):
-            raise ValueError("lower_distance must be finite.")
-        if self.lower_distance < 0.0:
-            raise ValueError("lower_distance must be non-negative.")
         object.__setattr__(self, "target_axis", self.target_axis.clone())
 
 
 class AxisAlign(AtomicAction[AxisAlignGoal, AxisAlignOptions]):
-    """Grasp an object, align its local axis to a world axis, and release it."""
+    """Grasp an object and align its local axis to a world axis."""
 
     skill_id: ClassVar[str] = "axis_align"
     GoalType: ClassVar[type] = AxisAlignGoal
@@ -181,7 +174,7 @@ def _plan(
         request: ResolvedActionRequest[AxisAlignGoal, AxisAlignOptions],
         context: PlanningContext,
     ) -> ActionPlan:
-        """Plan all seven physical actions in two arm-planning phases."""
+        """Plan grasp, lift, and alignment in two arm-planning phases."""
         target = request.goal
         options = request.skill_options
         affordance = self._require_axis_align_affordance(target.semantics)
@@ -301,7 +294,7 @@ def _plan(
         object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos)
         lifted_object_pose = torch.bmm(lift_xpos, pose_inv(object_to_eef))
 
-        n_approach, n_reach, n_lift, n_align, n_lower = self._motion_segment_lengths(
+        n_approach, n_reach, n_lift, n_align = self._motion_segment_lengths(
             request.motion_policy.sample_count,
             options.hand_interp_steps,
         )
@@ -317,14 +310,6 @@ def _plan(
             options.target_axis,
             waypoint_count=1,
         )
-        lower_xpos = translate_pose_world(
-            align_xpos[:, -1],
-            torch.tensor(
-                [0.0, 0.0, -options.lower_distance],
-                device=self.device,
-                dtype=torch.float32,
-            ),
-        )
 
         # CuRobo planning is grouped by gripper state.  The open-gripper phase
         # contains both the pre-grasp and grasp waypoints, so one generate call
@@ -339,21 +324,19 @@ def _plan(
             interpolation_dt,
         )
 
-        # Once the gripper is closed, lifting, alignment, and lowering form one
-        # continuous held-object phase.  Passing only those three semantic
+        # Once the gripper is closed, lifting and alignment form one continuous
+        # held-object phase.  Passing only those two semantic
         # endpoints retains the required ordering without expanding the rotation
         # into many CuRobo plan_pose calls.  Together with the open-gripper phase,
-        # the action now uses two MotionGenerator.generate calls and five backend
-        # target plans instead of n_align + 4 backend target plans.
-        post_close_xpos = torch.cat(
-            [lift_xpos[:, None], align_xpos, lower_xpos[:, None]], dim=1
-        )
+        # the action uses two MotionGenerator.generate calls and four backend
+        # target plans instead of n_align + 3 backend target plans.
+        post_close_xpos = torch.cat([lift_xpos[:, None], align_xpos], dim=1)
         post_close_success, post_close_arm = self._plan_pose_phase(
             post_close_xpos,
             pre_close_arm[:, -1],
             manipulator,
             request,
-            n_lift + n_align + n_lower,
+            n_lift + n_align,
             interpolation_dt,
         )
         success = grasp_success & normalize_success_mask(
@@ -368,16 +351,10 @@ def _plan(
             hand_grasp_qpos,
             n_waypoints=options.hand_interp_steps,
         )
-        hand_open = interpolate_hand_qpos(
-            hand_grasp_qpos,
-            hand_open_qpos,
-            n_waypoints=options.hand_interp_steps,
-        )
         segment_lengths = {
             "approach": pre_close_arm.shape[1],
             "close": hand_close.shape[1],
             "manipulate": post_close_arm.shape[1],
-            "open": hand_open.shape[1],
         }
         full = torch.empty(
             (self.num_envs, sum(segment_lengths.values()), self.robot_dof),
@@ -395,9 +372,6 @@ def _plan(
         stop = offset + post_close_arm.shape[1]
         full[:, offset:stop, arm_joint_ids] = post_close_arm
         full[:, offset:stop, hand_joint_ids] = hand_grasp_qpos.unsqueeze(1)
-        offset = stop
-        full[:, offset:, arm_joint_ids] = post_close_arm[:, -1].unsqueeze(1)
-        full[:, offset:, hand_joint_ids] = hand_open
 
         return self.build_plan(
             request,
@@ -606,16 +580,16 @@ def _apply_upright_grasp_pre_rotation(
     def _motion_segment_lengths(
         sample_count: int,
         hand_interp_steps: int,
-    ) -> tuple[int, int, int, int, int]:
-        motion_count = sample_count - 2 * hand_interp_steps
-        if motion_count < 5:
+    ) -> tuple[int, int, int, int]:
+        motion_count = sample_count - hand_interp_steps
+        if motion_count < 4:
             raise ValueError(
                 "Not enough waypoints for AxisAlign. Increase sample_count or "
                 "decrease hand_interp_steps."
             )
-        base, remainder = divmod(motion_count, 5)
-        values = [base + (index < remainder) for index in range(5)]
-        return values[0], values[1], values[2], values[3], values[4]
+        base, remainder = divmod(motion_count, 4)
+        values = [base + (index < remainder) for index in range(4)]
+        return values[0], values[1], values[2], values[3]
 
     @staticmethod
     def _require_axis_align_affordance(

scripts/tutorials/atomic_action/axis_align.py

@@ -145,7 +145,7 @@ def create_axis_align_semantics(
 
 
 def main() -> None:
-    """Plan and replay a grasp, axis alignment, lowering, and release."""
+    """Plan and replay a grasp followed by axis alignment."""
     args = parse_arguments()
     sim = create_tutorial_simulation(args)
     robot = add_ur5_gripper_robot(sim, tcp_z=0.15)
@@ -211,7 +211,6 @@ def main() -> None:
                     ),
                     pre_grasp_distance=0.15,
                     lift_height=0.16,
-                    lower_distance=0.03,
                     hand_interp_steps=HAND_INTERP_STEPS,
                 ),
             ),

tests/gym/envs/expert_program/test_configured_runtime.py

@@ -30,28 +30,23 @@
 import torch
 
 from embodichain.lab.gym.envs import EmbodiedEnv
-from embodichain.lab.gym.envs.expert_program import SequenceCfg, load_expert_program
+from embodichain.lab.gym.envs.expert_program import load_expert_program
 from embodichain.lab.gym.envs.expert_program._configured_runtime_services import (
     _MoveHeldObjectLowerer,
     _PourLowerer,
-    _PushObjectLowerer,
 )
 from embodichain.lab.gym.envs.expert_program.configured_runtime import (
     _decode_configured_expert_program_runtime,
     _decode_grasp_generator,
     _register_configured_expert_program_runtime,
 )
 from embodichain.lab.sim.atomic_actions import (
-    Affordance,
     HeldObjectPoseGoal,
     MoveHeldObjectOptions,
     PickUpOptions,
     PlaceOptions,
     PourGoal,
     PourOptions,
-    PushObjectGoal,
-    PushObjectOptions,
-    ObjectSemantics,
 )
 from embodichain.lab.sim.skills import RegisteredSemanticCall, SceneObjectRef
 from embodichain.lab.gym.utils.gym_utils import config_to_cfg
@@ -99,11 +94,6 @@
             }
         ),
     ),
-    "rearrangement": (
-        "expert_program_tableware_rearrangement",
-        "expert_program_cobotmagic_rearrangement",
-        frozenset({"pick", "place", "hand_over", "simulation.push_object"}),
-    ),
 }
 
 
@@ -366,109 +356,6 @@ def test_pick_option_rejects_malformed_fixed_object_to_eef() -> None:
         _decode_configured_expert_program_runtime(payload)
 
 
-def test_rearrangement_runs_two_arms_sequentially_until_parallel_is_safe() -> None:
-    """Planar pushing replaces unreliable thin-utensil pickup trajectories."""
-    config = _tableware_gym_config("rearrangement")
-    runtime = _decode_configured_expert_program_runtime(
-        config["expert_program_runtime"]
-    )
-    registration = runtime.registration
-    push_options = registration.robot_profile_binding.presets[
-        0
-    ].action_option_templates["simulation.push_object"]
-    program = load_expert_program(
-        config["expert_program_path"],
-        base_dir=_tableware_config_path("rearrangement").parent,
-        validation_context=registration.catalog,
-    )
-
-    assert type(program.program) is SequenceCfg
-    assert [segment.name for segment in program.program.items] == [
-        "push_fork_toward_plate_slot",
-        "refine_fork_at_plate_slot",
-        "push_spoon_toward_plate_slot",
-        "refine_spoon_at_plate_slot",
-    ]
-    assert type(push_options) is PushObjectOptions
-    assert push_options.hand_interp_steps == 7
-    assert push_options.approach_height == pytest.approx(0.1)
-    assert push_options.retract_height == pytest.approx(0.1)
-    assert push_options.contact_distance == pytest.approx(0.03)
-    assert push_options.push_overshoot == pytest.approx(0.02)
-    assert push_options.completion_tolerance == pytest.approx(0.075)
-    assert torch.equal(
-        push_options.object_contact_offset,
-        torch.tensor([-0.05, 0.0, 0.0]),
-    )
-    assert torch.equal(
-        push_options.support_frame_planar_contact_offset,
-        torch.tensor([-0.05, 0.0, 0.0]),
-    )
-    assert len(push_options.tool_calibrations) == 1
-    assert push_options.tool_calibrations[0].control_part == "right_arm"
-    assert push_options.tool_calibrations[0].contact_distance == pytest.approx(0.05)
-    assert registration.robot_profile_binding.presets[
-        0
-    ].recovery_policy.goal_rotation_threshold == pytest.approx(torch.pi)
-    assert registration.scene_binding.antipodal_grasps == ()
-    assert len(registration.registered_semantic_lowerer_factories) == 1
-    assert registration.registered_semantic_lowerer_factories[0].routes == (
-        ("fork", "plate_fork_slot"),
-        ("spoon", "plate_spoon_slot"),
-    )
-    assert (
-        "grasp_pose_generators"
-        not in config["expert_program_runtime"]["runtime_services"]
-    )
-
-
-def test_rearrangement_push_lowerer_builds_a_late_bound_typed_goal() -> None:
-    """Registered arguments choose only one predeclared object-target route."""
-    semantics = ObjectSemantics(
-        affordance=Affordance(),
-        geometry={},
-        entity_id="fork",
-        label="fork",
-    )
-    lowering = _PushObjectLowerer(
-        (("fork", "plate_fork_slot"),),
-        (semantics,),
-    ).lower(
-        RegisteredSemanticCall(
-            call_id="simulation.push_object",
-            arguments={"object": "fork", "target": "plate_fork_slot"},
-        ),
-        context=None,  # type: ignore[arg-type]
-        bound=None,  # type: ignore[arg-type]
-        option_template=PushObjectOptions(),
-    )
-
-    assert type(lowering.goal) is PushObjectGoal
-    assert lowering.goal.semantics is semantics
-    assert lowering.goal.target_pose.entity_id == "plate_fork_slot"
-
-
-def test_rearrangement_keeps_fixed_plate_out_of_interactive_objects() -> None:
-    """The kinematic placement reference belongs to the background scene."""
-    config = _tableware_gym_config("rearrangement")
-    background = {item["uid"]: item for item in config["background"]}
-    interactive_ids = {item["uid"] for item in config["rigid_object"]}
-
-    assert background["plate"]["body_type"] == "kinematic"
-    assert "plate" not in interactive_ids
-    assert interactive_ids == {"fork", "spoon"}
-
-
-def test_rearrangement_settles_thin_tableware_before_first_push() -> None:
-    """Thin resting utensils receive a full contact-settling window on reset."""
-    config = _tableware_gym_config("rearrangement")
-    settle = config["env"]["events"]["settle_tableware_on_reset"]
-
-    assert settle["mode"] == "reset"
-    assert settle["params"]["min_steps"] == 50
-    assert settle["params"]["timeout_behavior"] == "raise"
-
-
 @pytest.mark.parametrize("task_name", tuple(_TASKS))
 def test_all_examples_register_plain_embodied_env_under_config_selected_ids(
     task_name: str,

tests/gym/envs/test_official_task_layout.py

@@ -43,15 +43,14 @@
     "StayStillSave-v1": "embodichain_tasks.special.stay_still_save",
 }
 REMOVED_AGENT_ENV_IDS = {"PourWaterAgent-v3", "RearrangementAgent-v3"}
-CONFIG_DEFINED_EXPERT_TASKS = {"pour_water", "rearrangement"}
+CONFIG_DEFINED_EXPERT_TASKS = {"pour_water"}
 RL_SIMULATOR_ENV_IDS = {"CartPoleRL", "PushCubeRL"}
 TABLEWARE_CONFIG_TASKS = {
     "blocks_ranking_rgb",
     "blocks_ranking_size",
     "match_object_container",
     "place_object_drawer",
     "pour_water",
-    "rearrangement",
     "scoop_ice",
     "stack_blocks_two",
     "stack_cups",