Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 14 additions & 5 deletions agent_context/topics/env-framework/env-framework.md
Original file line number Diff line number Diff line change
Expand Up @@ -127,9 +127,11 @@ flat action validation/slicing → term processing → term application

Raw policy input is one ordered flat floating tensor. `ActionManager` validates
it before state changes, processes every slice, and applies each term's owned
resources. `ControllerAction` stays wrapped through preprocessing, bypasses the
manager entirely, and validates qpos/qvel/qf keys, batch, joint width, floating
dtype, and device at the direct controller boundary. Both paths retain the
resources. `ControllerAction` stays wrapped through preprocessing and normally validates
qpos/qvel/qf keys, batch, joint width, floating dtype and device at the direct
controller boundary. The position-only `expert_trajectory.use_action_manager`
opt-in instead processes joint targets through the configured policy terms,
retaining exact FK targets and expert recording schema. Both paths retain the
ordinary observation, reward, termination, and rollout lifecycle.

Read final task success before resetting scene/bridge state. A Task Program
Expand Down Expand Up @@ -200,7 +202,14 @@ the traceback can retain native resources not yet registered with the manager.

### Stateful sensor and action lifecycle

BaseEnv starts one simulation-manager update per control interval and samples
participating sensors through its `after_substep` observer. See the sensor-owned [contact history contract](../sensor-system/contact-history.md)
`BaseEnv._advance_physics()` starts one simulation-manager update per control
interval and samples participating sensors through its `after_substep` observer.
`EmbodiedEnv._advance_physics()` connects an ActionManager control-interval callback
to the batched simulation update after sensor sampling, and delegates ordinary
actions to the base implementation. ActionManager owns the substep count, term
refresh and inactive-row holds. Intermediate
recording and visualization frames retain the simulation manager's publication cadence.
Direct controller-ready commands bypass
this refresh path; the explicit expert ActionManager route participates. See the sensor-owned [contact history contract](../sensor-system/contact-history.md)
for interval accumulation, counterpart filtering and selective reset.
EmbodiedEnv resets action and event managers before applying reset-mode events.
14 changes: 13 additions & 1 deletion agent_context/topics/manager-functor/manager-functor.md
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,19 @@ The config attribute name is the stable functor identity.

For a flat policy tensor, the environment calls manager processing, manager
application, physics, event `interval`, observation, and reward in that order.
`ControllerAction` bypasses the manager and uses the direct controller boundary;
A term can opt into physics-substep refresh.
`ActionManager.get_substep_callback(num_substeps)` creates a callback scoped to
one control interval, or returns None when no refresh is needed. The manager
counts remaining steps, refreshes opted-in terms only before another physics
step, and preserves inactive demo rows' measured holds. The first command is
processed and applied normally. `EmbodiedEnv` connects the callback after sensor
sampling; simulation advancement and publication remain environment-owned.
`expert_trajectory.use_action_manager` routes position-only expert commands
through `ActionManager.process_joint_targets`. Relative substep DLS and binary
gripper terms support that route, retaining exact FK targets and the expert
storage schema while using the policy controller. Completed rows are masked
after target conversion so binary grippers also retain measured joint holds.
`ControllerAction` otherwise bypasses the manager for direct controller writes;
the full loop and reset ordering are owned by
[environment execution](../env-framework/execution.md).

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ functors), :class:`ActionManager` (``process_action(action)`` then
JointPositionToLimitsAction
JointVelocityAction
ParallelGripperAction
RelativeEefDlsAction
RelativeEefPoseAction
RelativeJointPositionAction
LeRobotRecorder
Expand Down Expand Up @@ -194,6 +195,34 @@ The action descriptor records its method, damping, bound, and success semantics.
``joint_delta``, ``joint_step_limited``, and ``joint_limit_clipped_mask`` expose
the applied increment and both forms of limiting; resets clear selected rows.

``RelativeEefDlsAction`` keeps an unbounded xyz/rotation-vector request as one
fixed arena-frame target for the control interval. It recomputes the damped
Jacobian solve from measured joints before each physics substep and clips only
joint-position limits. It shares solver-order mapping and arena/TCP Jacobian
adaptation with the single-step relative controller. Its ``eef_pose.relative_substep_dls@1`` contract selects
this behavior separately from the bounded, single-step relative-pose contract.
``ActionManager.get_substep_callback(num_substeps)`` owns the remaining-step
count, action refresh and inactive-row holds for one control interval.
``EmbodiedEnv`` connects that callback after sensor sampling through the batched
simulation update, preserving intermediate recording and visualization frames.

Configure the same arm and binary gripper terms for expert and policy execution.
Set ``env.expert_trajectory.use_action_manager: true`` to route position-only
``ControllerAction`` commands through ``ActionManager.process_joint_targets``.
The DLS term retains the expert's exact FK target while recording the
corresponding relative request, and keeps the existing expert storage schema.
Completed demo rows retain measured joint holds after conversion and refresh.
The default controller-ready route stays direct. Every configured term must
support joint-target processing.
For binary grippers, ``open_on_zero=False`` makes zero a closing request;
the default retains nonnegative opening requests. The descriptor records this
boundary together with the gripper mode.

.. autoclass:: RelativeEefDlsAction
:members:
:inherited-members:
:show-inheritance:

.. autoclass:: RelativeEefPoseAction
:members:
:inherited-members:
Expand Down
19 changes: 19 additions & 0 deletions docs/source/api_reference/public_api.rst
Original file line number Diff line number Diff line change
Expand Up @@ -1299,6 +1299,7 @@ embodichain.lab.gym.envs.managers.actions
JointPositionToLimitsAction
JointVelocityAction
ParallelGripperAction
RelativeEefDlsAction
RelativeEefPoseAction
RelativeJointPositionAction

Expand All @@ -1313,6 +1314,24 @@ embodichain.lab.gym.envs.managers.action_types
ActionTermDescriptor
ActionTrace

embodichain.lab.gym.envs.managers.observation_manager
-----------------------------------------------------

.. currentmodule:: embodichain.lab.gym.envs.managers.observation_manager

.. autosummary::

ObservationManager

embodichain.lab.gym.envs.managers.reward_manager
------------------------------------------------

.. currentmodule:: embodichain.lab.gym.envs.managers.reward_manager

.. autosummary::

RewardManager

embodichain.lab.gym.envs.objectives
----------------------------------

Expand Down
57 changes: 55 additions & 2 deletions embodichain/lab/gym/envs/embodied_env.py
Original file line number Diff line number Diff line change
Expand Up @@ -2473,6 +2473,10 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction:
"""

if isinstance(action, ControllerAction):
if getattr(self, "_step_uses_action_manager", False):
self.action_manager.apply_action()
self._last_action_trace = self.action_manager.action_trace()
return action.value
return EmbodiedEnv._apply_controller_action(self, action.value)
action_manager = getattr(self, "action_manager", None)
if action_manager is not None:
Expand All @@ -2481,6 +2485,33 @@ def _step_action(self, action: EnvAction | ControllerAction) -> EnvAction:
return action
return EmbodiedEnv._apply_controller_action(self, action)

def _advance_physics(self) -> None:
"""Connect manager callbacks to physics and sensor sampling."""
manager = getattr(self, "action_manager", None)
if not getattr(self, "_step_uses_action_manager", False) or manager is None:
return super()._advance_physics()
refresh_actions = manager.get_substep_callback(self.cfg.sim_steps_per_control)
if refresh_actions is None:
return super()._advance_physics()

sensors = [
sensor for sensor in self.sensors.values() if sensor.requires_substep_update
]
for sensor in sensors:
sensor.begin_control_step()

def observe_substep(dt: float) -> None:
for sensor in sensors:
sensor.update_physics_step(dt)
refresh_actions(dt)

self.sim.update(
self.physics_dt,
self.cfg.sim_steps_per_control,
after_substep=observe_substep,
render_final_step=False,
)

@property
def last_action_trace(self) -> ActionTrace | None:
"""Return the most recent ActionManager request/command trace."""
Expand Down Expand Up @@ -2618,6 +2649,17 @@ def _preprocess_action(
self._last_action_manager_qpos = None
self._last_action_trace = None
is_controller_action = isinstance(action, ControllerAction)
expert_cfg = getattr(getattr(self, "cfg", None), "expert_trajectory", None)
managed_expert = is_controller_action and bool(
getattr(expert_cfg, "use_action_manager", False)
)
if managed_expert and self.action_manager is None:
raise ValueError(
"ActionManager expert execution requires configured action terms."
)
self._step_uses_action_manager = self.action_manager is not None and (
not is_controller_action or managed_expert
)
controller_metadata = action.metadata if is_controller_action else None
policy_trajectory = self._traj_buffer is not None and (
getattr(self, "_trajectory_action_kind", None) == "policy"
Expand Down Expand Up @@ -2653,7 +2695,9 @@ def _preprocess_action(
if retain_raw_action
else action
)
if self.action_manager is not None and not is_controller_action:
if self.action_manager is not None and (
not is_controller_action or managed_expert
):
if policy_recording:
explicit_policy = getattr(self, "_explicit_policy_rollout", False)
if explicit_policy:
Expand All @@ -2664,7 +2708,16 @@ def _preprocess_action(
raise RuntimeError(
"Policy action recording requires an initialized DatasetManager."
)
self.action_manager.process_action(action)
if managed_expert:
action = self._prepare_controller_action(action)
if getattr(self, "_demo_no_auto_reset", False):
action = self._mask_controller_demo_action(action)
qpos = action if isinstance(action, torch.Tensor) else action["qpos"]
full_targets = self.robot.get_qpos().clone()
full_targets[:, self.active_joint_ids] = self._active_qpos_command(qpos)
self.action_manager.process_joint_targets(full_targets)
else:
self.action_manager.process_action(action)
if getattr(self, "_demo_no_auto_reset", False):
self.action_manager.mask_inactive(self._demo_active_mask)
if self.dataset_manager is not None and not policy_recording:
Expand Down
10 changes: 10 additions & 0 deletions embodichain/lab/gym/envs/expert_trajectory.py
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,23 @@ class ExpertTrajectoryCfg:

Args:
joint_command_mode: Position-only or position-velocity expert targets.
use_action_manager: Route position targets through configured policy terms.
"""

joint_command_mode: JointCommandMode = "position"
"""Joint targets emitted and stored by expert trajectory generation."""

use_action_manager: bool = False
"""Execute expert position targets through configured policy action terms."""

def __post_init__(self) -> None:
self.joint_command_mode = _validate_joint_command_mode(self.joint_command_mode)
if type(self.use_action_manager) is not bool:
raise ValueError("use_action_manager must be boolean.")
if self.use_action_manager and self.joint_command_mode != "position":
raise ValueError(
"ActionManager expert execution requires position-only targets."
)


@dataclass(frozen=True, slots=True)
Expand Down
1 change: 1 addition & 0 deletions embodichain/lab/gym/envs/managers/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@
"JointPositionToLimitsAction",
"JointVelocityAction",
"ParallelGripperAction",
"RelativeEefDlsAction",
"RelativeEefPoseAction",
"RelativeJointPositionAction",
"register_action_contract",
Expand Down
87 changes: 87 additions & 0 deletions embodichain/lab/gym/envs/managers/action_manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
from __future__ import annotations

from abc import abstractmethod
from collections.abc import Callable
from dataclasses import replace
from functools import cached_property
import inspect
Expand Down Expand Up @@ -50,6 +51,7 @@ class ActionTerm(Functor):
"""

contract_id: ClassVar[str | None] = None
requires_substep_update: ClassVar[bool] = False

def __init__(self, cfg: ActionTermCfg, env: EmbodiedEnv):
"""Initialize an action term.
Expand Down Expand Up @@ -78,6 +80,22 @@ def resolved_contract_id(self) -> str | None:
)
return self.cfg.contract or self.contract_id

def update_substep(self) -> None:
"""Refresh a processed command from current physical state."""

def process_joint_targets(self, qpos: torch.Tensor) -> None:
"""Process full-joint expert targets through this term's controller.

Args:
qpos: Full robot joint targets, shaped ``(num_envs, dof)``.

Raises:
NotImplementedError: If the term has no expert-target conversion.
"""
raise NotImplementedError(
f"{type(self).__name__} cannot process joint targets."
)

@property
@abstractmethod
def action_dim(self) -> int:
Expand Down Expand Up @@ -152,6 +170,10 @@ def __init__(self, cfg: object, env: EmbodiedEnv):
device=env.device,
)
self._previous_action = torch.zeros_like(self._action)
self._substep_terms = tuple(
term for term in self._terms.values() if term.requires_substep_update
)
self._substep_active_mask: torch.Tensor | None = None

def __str__(self) -> str:
"""Return a table describing the ordered flat action layout."""
Expand Down Expand Up @@ -194,6 +216,67 @@ def previous_action(self) -> torch.Tensor:
"""Return previous complete raw policy actions."""
return self._previous_action

def get_substep_callback(self, num_substeps: int) -> Callable[[float], None] | None:
"""Create action refresh scheduling for one control interval.

Args:
num_substeps: Positive number of physics steps in the interval.

Returns:
An after-substep callback, or None when no refresh is needed.
The initial command must already be applied; the callback refreshes
only when another physics step remains.

Raises:
ValueError: If num_substeps is less than one.
"""
if num_substeps < 1:
raise ValueError(f"Expected positive num_substeps, found {num_substeps}.")
if not self._substep_terms or num_substeps == 1:
return None
remaining = num_substeps

def refresh_actions(dt: float) -> None:
nonlocal remaining
del dt
remaining -= 1
if remaining <= 0:
return
for term in self._substep_terms:
term.update_substep()
if self._substep_active_mask is not None:
self.mask_inactive(self._substep_active_mask)
for term in self._substep_terms:
term.apply_actions()

return refresh_actions

def process_joint_targets(self, qpos: torch.Tensor) -> None:
"""Process expert targets while preserving exact FK target poses.

Args:
qpos: Finite full-joint position targets on the environment device.
"""
expected = self._env.robot.get_qpos().shape
if (
not isinstance(qpos, torch.Tensor)
or not qpos.is_floating_point()
or qpos.shape != expected
or qpos.device != torch.device(self._env.device)
or not bool(torch.isfinite(qpos).all())
):
raise ValueError(
f"Expected finite joint targets with shape {expected} on {self._env.device}."
)

self._previous_action.copy_(self._action)
self._substep_active_mask = None
for term in self._terms.values():
term.process_joint_targets(qpos)
self._action.copy_(
torch.cat([term.raw_actions for term in self._terms.values()], dim=-1)
)

@property
def descriptors(self) -> tuple[ActionDescriptor, ...]:
"""Return ordered action-term descriptors bound to flat slices."""
Expand Down Expand Up @@ -277,6 +360,7 @@ def process_action(self, action: torch.Tensor) -> None:
)

self._previous_action.copy_(self._action)
self._substep_active_mask = None
self._action.copy_(action)
for name, term in self._terms.items():
term.process_actions(self._action[:, self._slices[name]])
Expand Down Expand Up @@ -333,6 +417,7 @@ def mask_inactive(self, active_mask: torch.Tensor) -> None:
raise ValueError(
f"active_mask must be on {expected_device}, got {active_mask.device}."
)
self._substep_active_mask = active_mask.clone()

measured_qpos: torch.Tensor | None = None
for name, term in self._terms.items():
Expand Down Expand Up @@ -371,6 +456,8 @@ def reset(
ids = slice(None) if env_ids is None else env_ids
self._action[ids] = 0
self._previous_action[ids] = 0
if self._substep_active_mask is not None:
self._substep_active_mask[ids] = True
for term in self._terms.values():
term.reset(env_ids=env_ids)
return {}
Expand Down
Loading
Loading