diff --git a/docs/source/overview/sim/atomic_actions/builtin_actions.md b/docs/source/overview/sim/atomic_actions/builtin_actions.md
index 667789f43..d0bcb3c73 100644
--- a/docs/source/overview/sim/atomic_actions/builtin_actions.md
+++ b/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
@@ -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.
diff --git a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py b/embodichain/lab/sim/atomic_actions/primitives/axis_align.py
index a6bb4a543..3e7740a13 100644
--- a/embodichain/lab/sim/atomic_actions/primitives/axis_align.py
+++ b/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(
diff --git a/scripts/tutorials/atomic_action/axis_align.py b/scripts/tutorials/atomic_action/axis_align.py
index 483b5deda..f0fb8e3be 100644
--- a/scripts/tutorials/atomic_action/axis_align.py
+++ b/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,
),
),
diff --git a/tests/gym/envs/expert_program/test_configured_runtime.py b/tests/gym/envs/expert_program/test_configured_runtime.py
index 42eaf3a26..1c6034ae0 100644
--- a/tests/gym/envs/expert_program/test_configured_runtime.py
+++ b/tests/gym/envs/expert_program/test_configured_runtime.py
@@ -30,11 +30,10 @@
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,
@@ -42,16 +41,12 @@
_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,
diff --git a/tests/gym/envs/test_official_task_layout.py b/tests/gym/envs/test_official_task_layout.py
index 774ed3193..853238e6c 100644
--- a/tests/gym/envs/test_official_task_layout.py
+++ b/tests/gym/envs/test_official_task_layout.py
@@ -43,7 +43,7 @@
"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",
@@ -51,7 +51,6 @@
"match_object_container",
"place_object_drawer",
"pour_water",
- "rearrangement",
"scoop_ice",
"stack_blocks_two",
"stack_cups",
diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py
index 69f02fdc4..0152b1534 100644
--- a/tests/sim/atomic_actions/test_actions.py
+++ b/tests/sim/atomic_actions/test_actions.py
@@ -1694,7 +1694,6 @@ def compute_ik(
skill_options=AxisAlignOptions(
target_axis=torch.tensor([1.0, 0.0, 0.0]),
lift_height=0.1,
- lower_distance=0.03,
),
),
context,
@@ -1709,10 +1708,9 @@ def compute_ik(
"approach",
"close",
"manipulate",
- "open",
]
assert generator.generate.call_count == 2
- assert len(solved_poses) == 5
+ assert len(solved_poses) == 4
assert plan.expected_effects.is_empty
assert context.task is original_task
assert plan.scene_dependencies == ("target",)
@@ -1726,7 +1724,8 @@ def compute_ik(
torch.tensor([1.0, 0.0, 0.0]).expand(NUM_ENVS, -1),
atol=1.0e-6,
)
- assert solved_poses[-1][:, 2, 3].tolist() == pytest.approx([0.07, 0.07])
+ assert solved_poses[-1][:, 2, 3].tolist() == pytest.approx([0.1, 0.1])
+ assert torch.all(trajectory.positions[:, -1, ARM_DOF:] == 1.0)
def test_axis_align_upright_prefers_perpendicular_grasp_and_pre_rotates() -> None:
diff --git a/tests/test_agent_context_map.py b/tests/test_agent_context_map.py
index 0be58c662..e73b8d1b1 100644
--- a/tests/test_agent_context_map.py
+++ b/tests/test_agent_context_map.py
@@ -68,28 +68,6 @@ def test_topic_entries_use_the_required_schema() -> None:
assert errors == []
-def test_registered_context_and_source_paths_exist() -> None:
- context_map = _load_context_map()
- missing_paths: list[str] = []
-
- for context_path in context_map["defaults"]["contexts"]:
- resolved = _AGENT_CONTEXT_ROOT / context_path
- if not resolved.exists():
- missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT)))
-
- for topic in context_map["topics"]:
- for context_path in topic["paths"]:
- resolved = _AGENT_CONTEXT_ROOT / context_path
- if not resolved.exists():
- missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT)))
- for source_path in topic["source_of_truth"]:
- resolved = _REPOSITORY_ROOT / source_path
- if not resolved.exists():
- missing_paths.append(str(resolved.relative_to(_REPOSITORY_ROOT)))
-
- assert missing_paths == []
-
-
def test_related_topics_reference_registered_ids() -> None:
topics = _topics_by_id()
invalid_relations = [