atomic action by matafela · Pull Request #243 · DexForce/EmbodiChain

matafela · 2026-04-22T08:44:31Z

Description

Implement atomic action.
Fix qpos seed shape error in MotionGenerator.
Fix waypoint buliding error in ToppraPlanner
example: ‵scripts/tutorials/sim/atom_action.py`

TODO:

Place and Move action
comments and docs
unittest

Fixes # (issue)

Fix qpos seed shape error in MotionGenerator.
Fix waypoint buliding error in ToppraPlanner

Type of change

Bug fix (non-breaking change which fixes an issue)
Enhancement (non-breaking change which improves an existing functionality)
New feature (non-breaking change which adds functionality)

Checklist

I have run the black . command to format the code base.
I have made corresponding changes to the documentation
I have added tests that prove my fix is effective or that my feature works
Dependencies have been updated, if applicable.

Copilot

Pull request overview

This PR introduces an “atomic actions” workflow for simulation (engine + actions + semantics), adds a runnable tutorial script, and fixes two motion-planning issues (Toppra waypoint construction and MotionGenerator IK seed shaping).

Changes:

Add/extend atomic action engine semantics resolution (including dict “convenience” targets) and new action implementations (pick/place/move).
Fix planner bugs: Toppra waypoint building and MotionGenerator qpos_seed/batch-IK seed shaping.
Add a tutorial script and expand atomic-action unit tests.

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 16 comments.

Show a summary per file

File	Description
`embodichain/lab/sim/atomic_actions/core.py`	Refactors affordances/semantics and action base config; adds `AntipodalAffordance` and binds affordance geometry.
`embodichain/lab/sim/atomic_actions/engine.py`	Adds dict target resolution + enhanced `SemanticAnalyzer`; changes action instantiation to accept configs.
`embodichain/lab/sim/atomic_actions/actions.py`	Adds new Move/PickUp/Place actions and adjusts existing actions to new base init.
`embodichain/lab/sim/atomic_actions/__init__.py`	Updates exports (but currently inconsistent with removed `GraspPose`).
`embodichain/lab/sim/planners/motion_generator.py`	Fixes qpos seed fallback and passes correctly-shaped `joint_seed` into batch IK.
`embodichain/lab/sim/planners/toppra_planner.py`	Fixes waypoint building by converting qpos tensors to CPU numpy arrays.
`tests/sim/atomic_actions/test_core.py`	Adds new tests for affordance geometry/custom_config and dict-target convenience; still references removed `GraspPose`.
`scripts/tutorials/sim/atom_action.py`	Adds an example script demonstrating registration and execution of atomic actions.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-04-22T10:20:41Z

+        cfg: ActionCfg = ActionCfg(),
    ):


AtomicAction.__init__(..., cfg: ActionCfg = ActionCfg()) uses an instance as a default argument, so all actions created without an explicit cfg will share the same config object. Use cfg: ActionCfg | None = None and instantiate a fresh config inside __init__.

Suggested change

cfg: ActionCfg = ActionCfg(),

):

cfg: ActionCfg | None = None,

):

if cfg is None:

cfg = ActionCfg()

Copilot · 2026-04-22T10:20:41Z

+        approach_direction: torch.Tensor = torch.tensor(
+            [0, 0, -1], dtype=torch.float32
+        ),
+    ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]:
+        if self.generator is None:
+            self._init_generator()
+


get_best_grasp_poses() uses a default approach_direction=torch.tensor(...) created on CPU at import time. If obj_poses/the generator are on GPU, this can cause device-mismatch errors. Prefer approach_direction: torch.Tensor | None = None and create/to(obj_poses.device) inside the method.

Suggested change

approach_direction: torch.Tensor = torch.tensor(

[0, 0, -1], dtype=torch.float32

),

) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]:

if self.generator is None:

self._init_generator()

approach_direction: torch.Tensor | None = None,

) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]:

if self.generator is None:

self._init_generator()

if approach_direction is None:

approach_direction = torch.tensor(

[0, 0, -1], dtype=torch.float32, device=obj_poses.device

)

else:

approach_direction = approach_direction.to(

device=obj_poses.device, dtype=torch.float32

)

Copilot · 2026-04-22T10:20:42Z

+        grasp_affordance = AntipodalAffordance(
            object_label=label,
            poses=default_poses,
            grasp_types=["default"],
-        )
-
-        # Default interaction points
-        interaction_affordance = InteractionPoints(
-            object_label=label,
-            points=torch.zeros(1, 3),
-            point_types=["contact"],
+            custom_config=custom_config or {},


SemanticAnalyzer.analyze() constructs AntipodalAffordance with poses= and grasp_types=, but AntipodalAffordance doesn’t define those dataclass fields. This will raise TypeError when resolving a string/dict target. Either use an affordance type that supports poses (e.g. keep GraspPose) or update AntipodalAffordance’s API/fields accordingly.

Copilot · 2026-04-22T10:20:42Z

+def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]):
+    """


create_robot(..., position=[0.0, 0.0, 0.0]) uses a mutable list as a default argument. If the list is ever mutated, the default will be shared across calls. Use an immutable tuple (or None + assign inside) instead.

Copilot · 2026-04-22T10:20:42Z

        self,
        robot: "Robot",
        motion_generator: "MotionGenerator",
        device: torch.device = torch.device("cuda"),
+        actions_cfg_dict: Optional[Dict[str, ActionCfg]] = dict(),


actions_cfg_dict: Optional[Dict[str, ActionCfg]] = dict() is a mutable default argument and will be shared across AtomicActionEngine instances. Use None as the default and replace with a new {} inside __init__.

Copilot · 2026-04-22T10:20:44Z

 from embodichain.lab.sim.atomic_actions import (
    Affordance,
    GraspPose,
    InteractionPoints,
    ObjectSemantics,


This test module imports and exercises GraspPose, but this PR removes GraspPose from the embodichain.lab.sim.atomic_actions public surface. Tests will fail to import/run until they’re updated to the new affordance type (e.g. AntipodalAffordance) or GraspPose is restored.

Copilot · 2026-04-22T10:20:44Z

    label: str = "none"
    """Object category label (e.g., 'apple', 'bottle')."""

-    uid: Optional[str] = None
-    """Optional unique identifier for the object instance."""
+    entity: BatchEntity | None = None
+    """Optional reference to the underlying simulation entity representing this object."""


ObjectSemantics no longer defines a uid, but other new code in this PR (e.g. AtomicActionEngine._resolve_target) and tests still set/read semantics.uid. This will raise at runtime. Reintroduce uid: Optional[str] = None on ObjectSemantics (and keep it stable in __post_init__).

Copilot · 2026-04-22T10:20:44Z

+
+        self.n_envs = self.robot.get_qpos().shape[0]
+        self.arm_joint_ids = self.robot.get_joint_ids(name=self.cfg.control_part)
+        self.dof = len(self.arm_joint_ids)


ReachAction.__init__ still accepts control_part/device (and robot) but the refactor now calls super().__init__(motion_generator) and doesn’t apply these arguments. This makes ReachAction silently use the default ActionCfg.control_part instead of the requested control_part. Either remove the unused params or update the init path to set self.control_part/config consistently.

Suggested change

self.dof = len(self.arm_joint_ids)

super().__init__(motion_generator)

self.robot = robot

self.control_part = control_part

self.device = device

Copilot · 2026-04-22T10:20:44Z

+)
+from embodichain.lab.sim.planners import MotionGenerator, MotionGenCfg, ToppraPlannerCfg
+import os
+from embodichain.lab.sim.shapes import MeshCfg, CubeCfg


MeshCfg is imported twice (once at line 31 and again combined at line 45), and CubeCfg appears unused. Remove the duplicate import(s) to avoid confusion and potential lint failures.

Suggested change

from embodichain.lab.sim.shapes import MeshCfg, CubeCfg

Copilot · 2026-04-22T10:20:45Z

+import open3d as o3d
+import torch
+
+from embodichain.lab.sim import SimulationManager, SimulationManagerCfg
+from embodichain.lab.sim.objects import Robot, RigidObject
+from embodichain.lab.sim.utility.action_utils import interpolate_with_distance
+from embodichain.lab.sim.shapes import MeshCfg
+from embodichain.lab.sim.solvers import PytorchSolverCfg
+from embodichain.data import get_data_path
+from embodichain.utils import logger


This tutorial script has several imports that appear unused (open3d as o3d, interpolate_with_distance, logger). If the repo enforces unused-import checks, this file will fail linting. Remove unused imports to keep the example minimal.

Suggested change

import open3d as o3d

import torch

from embodichain.lab.sim import SimulationManager, SimulationManagerCfg

from embodichain.lab.sim.objects import Robot, RigidObject

from embodichain.lab.sim.utility.action_utils import interpolate_with_distance

from embodichain.lab.sim.shapes import MeshCfg

from embodichain.lab.sim.solvers import PytorchSolverCfg

from embodichain.data import get_data_path

from embodichain.utils import logger

import torch

from embodichain.lab.sim import SimulationManager, SimulationManagerCfg

from embodichain.lab.sim.objects import Robot, RigidObject

from embodichain.lab.sim.shapes import MeshCfg

from embodichain.lab.sim.solvers import PytorchSolverCfg

from embodichain.data import get_data_path

chenjian added 2 commits April 21, 2026 18:52

Bug: motion generate failed

6f3a2c2

implement pick up action

c2c8f69

matafela marked this pull request as draft April 22, 2026 08:44

chenjian added 2 commits April 22, 2026 17:43

add place action

ef67125

add move action

dbd28be

matafela requested review from Copilot and yuecideng April 22, 2026 10:10

Copilot started reviewing on behalf of matafela April 22, 2026 10:11 View session

style

e0f1264

Copilot AI reviewed Apr 22, 2026

View reviewed changes

chenjian added 5 commits April 22, 2026 18:26

update comments

7f4e4df

update

820010a

update

768edb9

add unittest

eced4f3

update

399193c

matafela marked this pull request as ready for review April 22, 2026 11:37

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

atomic action#243

atomic action#243
matafela wants to merge 10 commits intoyueci/atomic-action-initfrom
cj/atomic-action-init

matafela commented Apr 22, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Copilot AI Apr 22, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

-        cfg: ActionCfg = ActionCfg(),
-    ):
+        cfg: ActionCfg | None = None,
+    ):
+        if cfg is None:
+            cfg = ActionCfg()

		def create_robot(sim: SimulationManager, position=[0.0, 0.0, 0.0]):
		"""

-        self.dof = len(self.arm_joint_ids)
+        super().__init__(motion_generator)
+        self.robot = robot
+        self.control_part = control_part
+        self.device = device

Conversation

matafela commented Apr 22, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Fixes # (issue)

Type of change

Checklist

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 22, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

matafela commented Apr 22, 2026 •

edited

Loading