From 7f5f78515b7479687d3387d7161b23e4701d38df Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 15:57:34 +0000 Subject: [PATCH 1/3] feat(task-program): configure planner from execution policy --- .../topics/task-programs/configuration.md | 6 +- .../integrations/_configured_composition.py | 58 +++++- .../task_program/integrations/configured.py | 13 +- .../integrations/simulation/environment.py | 187 +++++++++++++++++- .../motion_gen_verified.yaml | 4 + .../test_configured_integration.py | 27 +++ .../test_simulation_environment.py | 43 ++++ 7 files changed, 326 insertions(+), 12 deletions(-) diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 4e9c5c9ab..990063f16 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -72,8 +72,10 @@ Ownership is explicit rather than a generic deep merge: - `configs/components/embodiments/*.yaml` owns simulation robot construction, the sensor suite, and an optional `skill_profile` containing logical resources/endpoints, command presets, and embodiment-specific services; -- `configs/components/execution_policies/*.yaml` owns motion, tracking, - recovery, runner, and effect-assurance policy. +- `configs/components/execution_policies/*.yaml` owns planner + selection/configuration, motion, tracking, recovery, runner, and + effect-assurance policy. Planner collision objects are attached from the + selected live scene and are not serialized in the policy. The reference embodiment `skill_profile.contract_id` and `profile_id` values are unversioned. Versioned Gym, task-integration, or scene-registry IDs are diff --git a/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 425fb692d..a527fb84b 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -56,6 +56,7 @@ "force_refresh", } ) +_PLANNER_TYPES = frozenset({"curobo", "neural", "toppra", "trapezoidal"}) @dataclass(frozen=True, slots=True) @@ -68,6 +69,7 @@ class _ConfiguredTaskProgramDeployment: selection: TaskProgramIntegrationCfg integration: _ConfiguredTaskProgramIntegration scene_binding: dict[str, object] + planner_config: dict[str, object] | None def _component_path( @@ -113,6 +115,35 @@ def _load_yaml_component(path: Path, *, field_name: str) -> dict[str, object]: return _owned_mapping(load_config(path), path=field_name) +def _decode_planner_config(value: object, *, path: str) -> dict[str, object]: + """Decode an executable-free planner selection from an execution policy.""" + config = _mapping( + value, + path=path, + required=frozenset({"type"}), + optional=frozenset({"config"}), + ) + planner_type = _identifier(config["type"], path=f"{path}.type") + if planner_type not in _PLANNER_TYPES: + raise ValueError( + f"{path}.type must be one of {sorted(_PLANNER_TYPES)}, " + f"got {planner_type!r}." + ) + options = _mapping( + config.get("config", {}), + path=f"{path}.config", + required=frozenset(), + ) + planner_config = { + "type": planner_type, + "config": deepcopy(dict(options)), + } + from .simulation.environment import _planner_cfg_from_config + + _planner_cfg_from_config(planner_config, robot_uid="__configured_policy__") + return planner_config + + def _merge_runtime_services( skill_profile_services: object, task_services: object, @@ -270,9 +301,23 @@ def _resolve_task_program_components( "effect_assurance", } ), - optional=frozenset({"required_planner"}), + optional=frozenset({"required_planner", "planner"}), ) _identifier(policy["policy_id"], path="execution policy.policy_id") + if "planner" in policy: + policy["planner"] = _decode_planner_config( + policy["planner"], + path="execution policy.planner", + ) + required_planner = policy.get("required_planner") + if ( + required_planner is not None + and required_planner != policy["planner"]["type"] + ): + raise ValueError( + "execution policy.required_planner must match " + "execution policy.planner.type when both are declared." + ) return program_path, integration, policy @@ -347,7 +392,9 @@ def _compose_integration_payload( "effect_assurance": deepcopy(policy["effect_assurance"]), "effect_monitors": deepcopy(profile["effect_monitors"]), } - if "required_planner" in policy: + if "planner" in policy: + preset["required_planner"] = deepcopy(policy["planner"]["type"]) + elif "required_planner" in policy: preset["required_planner"] = deepcopy(policy["required_planner"]) scene_payload = { @@ -416,7 +463,11 @@ def _load_configured_task_program_deployment( skill_profile=selected_skill_profile, scene=scene_binding, ) - integration = _decode_configured_task_program_integration(payload) + planner_config = deepcopy(policy.get("planner")) + integration = _decode_configured_task_program_integration( + payload, + planner_config=planner_config, + ) integration_id = _identifier( task["integration_id"], path="task integration.integration_id", @@ -440,4 +491,5 @@ def _load_configured_task_program_deployment( selection=selection, integration=integration, scene_binding=deepcopy(dict(scene_binding)), + planner_config=planner_config, ) diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index c80cb0538..666b15827 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -2185,8 +2185,17 @@ class _ConfiguredTaskProgramIntegration: def _decode_configured_task_program_integration( value: object, + *, + planner_config: Mapping[str, object] | None = None, ) -> _ConfiguredTaskProgramIntegration: - """Decode one composable, callable-free Task Program integration.""" + """Decode one composable, callable-free Task Program integration. + + Args: + value: Provider-free scene, robot-profile, and runtime-service payload. + planner_config: Optional execution-policy planner declaration retained + for the live simulation adapter. It is deliberately kept outside + the semantic integration payload. + """ path = "integration" config = _mapping( value, @@ -2225,6 +2234,7 @@ def _decode_configured_task_program_integration( fingerprint_payload = { "registration": registration.fingerprint, "grasp_pose_generators": grasp_fingerprint, + "planner_config": deepcopy(planner_config), } integration_fingerprint = hashlib.sha256( json.dumps( @@ -2238,6 +2248,7 @@ def _decode_configured_task_program_integration( delegate = SimulationTaskProgramAdapterFactory( registration, grasp_pose_generator_factories=dict(services.grasp_pose_generators), + planner_config=planner_config, ) adapter_factory = _ConfiguredTaskProgramAdapterFactory( delegate=delegate, diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 2e88bd2f5..87da86265 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -52,7 +52,15 @@ TaskState, ) from embodichain.lab.sim.motion.motion_generator import MotionGenCfg, MotionGenerator -from embodichain.lab.sim.motion.planners import BasePlannerCfg, ToppraPlannerCfg +from embodichain.lab.sim.motion.planners import ( + BasePlannerCfg, + CuroboAutoGenCfg, + CuroboPlannerCfg, + CuroboWorldCfg, + NeuralPlannerCfg, + ToppraPlannerCfg, + TrapezoidalPlannerCfg, +) from embodichain.lab.task_program.compiler.lowering import ( RegisteredSemanticLowerer, ) @@ -67,6 +75,8 @@ ) from embodichain.lab.task_program.semantics.profiles import RobotSkillProfile from embodichain.lab.task_program.semantics.scene import ( + SceneCollisionRole, + SceneCollisionWorldMode, RegistrySceneProvider, SceneRegistry, ) @@ -92,6 +102,87 @@ MotionGeneratorFactory = Callable[[], MotionGenerator] """Zero-argument factory that must return one fresh motion generator.""" +_PLANNER_CFG_TYPES: Mapping[str, type[BasePlannerCfg]] = { + "curobo": CuroboPlannerCfg, + "neural": NeuralPlannerCfg, + "toppra": ToppraPlannerCfg, + "trapezoidal": TrapezoidalPlannerCfg, +} + + +def _planner_cfg_from_config( + value: Mapping[str, object] | None, + *, + robot_uid: str, +) -> BasePlannerCfg | None: + """Build one typed planner config from an execution-policy declaration. + + The robot UID and simulation-manager instance are runtime-owned values and + are deliberately excluded from the serialized policy. cuRobo's live + collision objects are likewise attached by the simulation factory after + the selected environment has been initialized. + """ + if value is None: + return None + if not isinstance(value, Mapping): + raise TypeError("planner_config must be a mapping or None.") + unknown = sorted(set(value).difference({"type", "config"})) + if unknown: + raise ValueError(f"planner_config contains unsupported fields: {unknown}.") + planner_type = value.get("type") + if type(planner_type) is not str or not planner_type: + raise ValueError("planner_config.type must be a non-empty string.") + cfg_type = _PLANNER_CFG_TYPES.get(planner_type) + if cfg_type is None: + raise ValueError( + f"planner_config.type {planner_type!r} is unsupported; " + f"supported types are {sorted(_PLANNER_CFG_TYPES)}." + ) + options = value.get("config", {}) + if not isinstance(options, Mapping): + raise TypeError("planner_config.config must be a mapping.") + options = deepcopy(dict(options)) + reserved = sorted( + {"planner_type", "robot_uid", "sim_instance_id"}.intersection(options) + ) + if reserved: + raise ValueError( + "planner_config.config cannot override runtime-owned fields: " + f"{reserved}." + ) + + if planner_type == "curobo": + world = options.get("world") + if world is not None: + if not isinstance(world, Mapping): + raise TypeError("planner_config.config.world must be a mapping.") + world = deepcopy(dict(world)) + if "rigid_objects" in world: + raise ValueError( + "planner_config.config.world.rigid_objects is runtime-owned; " + "declare scene collision roles instead." + ) + if "dynamic_obstacle_names" in world: + raise ValueError( + "planner_config.config.world.dynamic_obstacle_names is " + "runtime-owned; declare dynamic scene collision roles instead." + ) + options["world"] = CuroboWorldCfg(**world) + auto_gen = options.get("auto_gen") + if auto_gen is not None: + if not isinstance(auto_gen, Mapping): + raise TypeError("planner_config.config.auto_gen must be a mapping.") + options["auto_gen"] = CuroboAutoGenCfg(**deepcopy(dict(auto_gen))) + + try: + planner_cfg = cfg_type(robot_uid=robot_uid, **options) + except (TypeError, ValueError) as exc: + raise ValueError( + f"Invalid planner_config for planner type {planner_type!r}: {exc}" + ) from exc + planner_cfg.validate() + return planner_cfg + class SimulationTaskProgramEnvironment(Protocol): """Minimal Gym environment surface used by the simulation factory.""" @@ -415,6 +506,9 @@ class SimulationTaskProgramFactory(TaskProgramEnvironmentFactory): joint_command_mode: Environment-owned expert joint command mode. planner_cfg: Explicit planner configuration. ``None`` selects TOPPRA for ``robot.uid``. + planner_config: Optional executable-free planner declaration from an + execution policy. It is converted to ``planner_cfg`` after the + selected runtime robot is known. motion_generator_factory: Optional fresh-generator factory. It is mutually exclusive with ``planner_cfg`` and intended for custom planners and isolated tests. @@ -533,6 +627,7 @@ def from_environment( *, registration: SimulationTaskProgramRegistration, planner_cfg: BasePlannerCfg | None = None, + planner_config: Mapping[str, object] | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, grasp_pose_generators: Mapping[str, GraspPoseGenerator] | None = None, translation_threshold: float = 1.0e-4, @@ -547,6 +642,13 @@ def from_environment( raise TypeError("environment must expose step_dt.") from exc if simulation is None or robot is None: raise TypeError("environment must expose non-None sim and robot values.") + if planner_cfg is not None and planner_config is not None: + raise ValueError("planner_cfg and planner_config are mutually exclusive.") + if planner_config is not None: + planner_cfg = _planner_cfg_from_config( + planner_config, + robot_uid=_robot_uid(robot), + ) expert_trajectory_cfg = getattr( getattr(environment, "cfg", None), "expert_trajectory", @@ -785,6 +887,8 @@ def _create_motion_generator(self) -> MotionGenerator: else deepcopy(self._planner_cfg) ) planner_cfg.sim_instance_id = self._simulation.instance_id + if isinstance(planner_cfg, CuroboPlannerCfg): + self._bind_curobo_scene(planner_cfg) generator = MotionGenerator(MotionGenCfg(planner_cfg=planner_cfg)) if not isinstance(generator, MotionGenerator): raise TypeError( @@ -792,6 +896,56 @@ def _create_motion_generator(self) -> MotionGenerator: ) return generator + def _bind_curobo_scene(self, planner_cfg: CuroboPlannerCfg) -> None: + """Attach live scene collision objects to a configured cuRobo planner.""" + if planner_cfg.world.rigid_objects is not None: + return + collision_bindings = tuple( + binding + for binding in self._scene_binding.rigid_objects + if binding.collision_role is not SceneCollisionRole.NONE + ) + unsupported_bindings = tuple( + binding.entity_id + for bindings in ( + self._scene_binding.rigidized_articulations, + self._scene_binding.articulations, + self._scene_binding.links, + ) + for binding in bindings + if binding.collision_role is not SceneCollisionRole.NONE + ) + if unsupported_bindings: + raise ValueError( + "Configured cuRobo collision worlds support only rigid-object " + "scene bindings; unsupported collision entities: " + f"{sorted(unsupported_bindings)}." + ) + + objects: dict[str, object] = {} + get_rigid_object = getattr(self._simulation, "get_rigid_object", None) + if collision_bindings and not callable(get_rigid_object): + raise TypeError("simulation must provide get_rigid_object().") + for binding in collision_bindings: + entity = get_rigid_object(binding.simulation_uid) + if entity is None: + raise KeyError( + f"Simulation UID {binding.simulation_uid!r} selected for " + f"collision entity {binding.entity_id!r} was not found." + ) + objects[binding.entity_id] = entity + + world = planner_cfg.world + world.rigid_objects = objects or None + world.dynamic_obstacle_names = [ + binding.entity_id + for binding in collision_bindings + if binding.collision_role is SceneCollisionRole.DYNAMIC + ] + collision_mode = self._scene_binding.collision_world_mode + if collision_mode is not None: + world.multi_env = collision_mode is SceneCollisionWorldMode.PER_ENV + @dataclass(frozen=True, slots=True, init=False) class SimulationTaskProgramAdapterFactory: @@ -801,6 +955,8 @@ class SimulationTaskProgramAdapterFactory: registration: Immutable provider-free task integration. grasp_pose_generator_factories: Zero-argument factories keyed by runtime grasp endpoint target ID. Each environment receives fresh services. + planner_config: Optional execution-policy planner declaration. The live + factory supplies the selected robot and simulation instance values. """ _registration: SimulationTaskProgramRegistration @@ -808,6 +964,7 @@ class SimulationTaskProgramAdapterFactory: str, Callable[[], GraspPoseGenerator], ] + _planner_config: Mapping[str, object] | None def __init__( self, @@ -816,6 +973,7 @@ def __init__( grasp_pose_generator_factories: ( Mapping[str, Callable[[], GraspPoseGenerator]] | None ) = None, + planner_config: Mapping[str, object] | None = None, ) -> None: if type(registration) is not SimulationTaskProgramRegistration: raise TypeError( @@ -826,6 +984,8 @@ def __init__( Mapping, ): raise TypeError("grasp_pose_generator_factories must be a mapping or None.") + if planner_config is not None and not isinstance(planner_config, Mapping): + raise TypeError("planner_config must be a mapping or None.") factories: dict[str, Callable[[], GraspPoseGenerator]] = {} for target_id, factory in (grasp_pose_generator_factories or {}).items(): if ( @@ -848,6 +1008,15 @@ def __init__( "_grasp_pose_generator_factories", MappingProxyType(factories), ) + object.__setattr__( + self, + "_planner_config", + ( + None + if planner_config is None + else MappingProxyType(deepcopy(dict(planner_config))) + ), + ) @property def registration(self) -> SimulationTaskProgramRegistration: @@ -871,11 +1040,13 @@ def create_adapter( "GraspPoseGenerator instance." ) generators[target_id] = generator - return create_simulation_task_program_adapter( - environment, - registration=self._registration, - grasp_pose_generators=generators, - ) + kwargs: dict[str, object] = { + "registration": self._registration, + "grasp_pose_generators": generators, + } + if self._planner_config is not None: + kwargs["planner_config"] = self._planner_config + return create_simulation_task_program_adapter(environment, **kwargs) def create_simulation_task_program_adapter( @@ -883,6 +1054,7 @@ def create_simulation_task_program_adapter( *, registration: SimulationTaskProgramRegistration, planner_cfg: BasePlannerCfg | None = None, + planner_config: Mapping[str, object] | None = None, motion_generator_factory: MotionGeneratorFactory | None = None, grasp_pose_generators: Mapping[str, GraspPoseGenerator] | None = None, translation_threshold: float = 1.0e-4, @@ -899,6 +1071,8 @@ def create_simulation_task_program_adapter( ``robot``, and ``step_dt``. registration: Required immutable task registration. planner_cfg: Optional planner configuration owned by the factory. + planner_config: Optional executable-free planner declaration from an + execution policy. motion_generator_factory: Optional factory for one fresh motion generator. grasp_pose_generators: Standalone grasp-pose services keyed by grasp endpoint target ID. @@ -912,6 +1086,7 @@ def create_simulation_task_program_adapter( environment, registration=registration, planner_cfg=planner_cfg, + planner_config=planner_config, motion_generator_factory=motion_generator_factory, grasp_pose_generators=grasp_pose_generators, translation_threshold=translation_threshold, diff --git a/embodichain_tasks/configs/components/execution_policies/motion_gen_verified.yaml b/embodichain_tasks/configs/components/execution_policies/motion_gen_verified.yaml index 60373330d..39fe216df 100644 --- a/embodichain_tasks/configs/components/execution_policies/motion_gen_verified.yaml +++ b/embodichain_tasks/configs/components/execution_policies/motion_gen_verified.yaml @@ -4,6 +4,10 @@ preset_id: safe requires: embodiment_contract: dual_arm_parallel_gripper +planner: + type: toppra + config: {} + motion: strategy: motion_gen sample_count: 140 diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 20b8b9313..52c1a75bc 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -40,6 +40,7 @@ ) from embodichain.lab.task_program.integrations._configured_composition import ( _compose_integration_payload, + _decode_planner_config, _load_configured_task_program_deployment, _resolve_task_program_components, ) @@ -252,6 +253,32 @@ def test_all_examples_decode_through_one_composable_integration_schema( assert integration.adapter_factory.registration is registration +def test_execution_policy_planner_config_reaches_configured_adapter() -> None: + """The policy-owned planner selection stays outside semantic payloads.""" + deployment = _deployment_from_path( + _CONFIG_DIRECTORY / "hand_over" / "task.dual_ur5_dh_pgi_140_80.yaml" + ) + + assert deployment.planner_config == {"type": "toppra", "config": {}} + delegate = deployment.integration.adapter_factory.delegate + assert delegate._planner_config == deployment.planner_config + assert ( + deployment.integration.registration.robot_profile_binding.presets[ + 0 + ].required_planner + == "toppra" + ) + + +def test_planner_policy_decoder_rejects_unknown_backend() -> None: + """Planner backends remain an explicit closed configuration set.""" + with pytest.raises(ValueError, match="execution policy.planner.type"): + _decode_planner_config( + {"type": "custom_callable", "config": {}}, + path="execution policy.planner", + ) + + @pytest.mark.parametrize("task_name", ("repeated_pick_place", "open_drawer")) def test_single_task_program_composes_with_ur5_and_franka( task_name: str, diff --git a/tests/gym/envs/task_program/test_simulation_environment.py b/tests/gym/envs/task_program/test_simulation_environment.py index 3b91f3e38..6b7ac7fcb 100644 --- a/tests/gym/envs/task_program/test_simulation_environment.py +++ b/tests/gym/envs/task_program/test_simulation_environment.py @@ -1531,6 +1531,49 @@ def _create_adapter(bound_environment: object, **kwargs: object): } +def test_adapter_factory_forwards_policy_planner_config( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """Execution-policy planner data reaches the live simulation factory.""" + registration = SimulationTaskProgramRegistration( + SimulationSceneBinding(registry_id="scene"), + _profile_binding(), + ) + planner_config = {"type": "toppra", "config": {"max_workers": 2}} + factory = SimulationTaskProgramAdapterFactory( + registration, + planner_config=planner_config, + ) + captured: dict[str, object] = {} + + def _create_adapter(bound_environment: object, **kwargs: object): + captured["environment"] = bound_environment + captured.update(kwargs) + return object.__new__(TaskProgramEnvironmentAdapter) + + monkeypatch.setattr( + simulation_environment_module, + "create_simulation_task_program_adapter", + _create_adapter, + ) + + factory.create_adapter(object()) + + assert captured["planner_config"] == planner_config + + +def test_planner_config_decodes_to_typed_runtime_cfg() -> None: + """A serialized planner selection becomes the selected planner config type.""" + planner_cfg = simulation_environment_module._planner_cfg_from_config( + {"type": "toppra", "config": {"max_workers": 2}}, + robot_uid="robot", + ) + + assert isinstance(planner_cfg, simulation_environment_module.ToppraPlannerCfg) + assert planner_cfg.robot_uid == "robot" + assert planner_cfg.max_workers == 2 + + def test_standard_registration_owns_and_freezes_live_runtime_assembly() -> None: """The standard path preserves exact ownership through live assembly.""" robot = _Robot() From f79e7201897c0eba2ee63d7eb07f10e10aeeba31 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 16:21:10 +0000 Subject: [PATCH 2/3] fix(task-program): preserve configured planner options --- .../integrations/_configured_composition.py | 8 ++-- .../integrations/simulation/environment.py | 15 +++++-- .../test_configured_integration.py | 9 ++++- .../test_simulation_environment.py | 39 ++++++++++++++++++- 4 files changed, 59 insertions(+), 12 deletions(-) diff --git a/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index a527fb84b..7c9e9b5a2 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -129,11 +129,9 @@ def _decode_planner_config(value: object, *, path: str) -> dict[str, object]: f"{path}.type must be one of {sorted(_PLANNER_TYPES)}, " f"got {planner_type!r}." ) - options = _mapping( - config.get("config", {}), - path=f"{path}.config", - required=frozenset(), - ) + options = config.get("config", {}) + if not isinstance(options, Mapping): + raise TypeError(f"{path}.config must be a mapping.") planner_config = { "type": planner_type, "config": deepcopy(dict(options)), diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 87da86265..525261793 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -33,7 +33,7 @@ from collections.abc import Callable, Iterable, Mapping from copy import deepcopy -from dataclasses import dataclass +from dataclasses import dataclass, fields import math from types import MappingProxyType from typing import Protocol, TYPE_CHECKING @@ -936,15 +936,22 @@ def _bind_curobo_scene(self, planner_cfg: CuroboPlannerCfg) -> None: objects[binding.entity_id] = entity world = planner_cfg.world - world.rigid_objects = objects or None - world.dynamic_obstacle_names = [ + dynamic_obstacle_names = [ binding.entity_id for binding in collision_bindings if binding.collision_role is SceneCollisionRole.DYNAMIC ] + world_values = { + field.name: getattr(world, field.name) for field in fields(CuroboWorldCfg) + } + world_values["rigid_objects"] = objects or None + world_values["dynamic_obstacle_names"] = dynamic_obstacle_names collision_mode = self._scene_binding.collision_world_mode if collision_mode is not None: - world.multi_env = collision_mode is SceneCollisionWorldMode.PER_ENV + world_values["multi_env"] = ( + collision_mode is SceneCollisionWorldMode.PER_ENV + ) + planner_cfg.world = CuroboWorldCfg(**world_values) @dataclass(frozen=True, slots=True, init=False) diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 52c1a75bc..26d21e757 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -270,8 +270,13 @@ def test_execution_policy_planner_config_reaches_configured_adapter() -> None: ) -def test_planner_policy_decoder_rejects_unknown_backend() -> None: - """Planner backends remain an explicit closed configuration set.""" +def test_planner_policy_decoder_validates_backend_and_options() -> None: + """Planner backends remain closed while typed options stay configurable.""" + assert _decode_planner_config( + {"type": "toppra", "config": {"max_workers": 2}}, + path="execution policy.planner", + ) == {"type": "toppra", "config": {"max_workers": 2}} + with pytest.raises(ValueError, match="execution policy.planner.type"): _decode_planner_config( {"type": "custom_callable", "config": {}}, diff --git a/tests/gym/envs/task_program/test_simulation_environment.py b/tests/gym/envs/task_program/test_simulation_environment.py index 6b7ac7fcb..349c81b3c 100644 --- a/tests/gym/envs/task_program/test_simulation_environment.py +++ b/tests/gym/envs/task_program/test_simulation_environment.py @@ -138,7 +138,10 @@ SemanticExecutionResult, SemanticExecutionStatus, ) -from embodichain.lab.task_program.semantics.scene import SceneObjectRef +from embodichain.lab.task_program.semantics.scene import ( + SceneCollisionRole, + SceneObjectRef, +) _BATCH_SIZE = 3 _ROBOT_DOF = 2 @@ -1574,6 +1577,40 @@ def test_planner_config_decodes_to_typed_runtime_cfg() -> None: assert planner_cfg.max_workers == 2 +def test_curobo_scene_binding_preserves_live_object_references() -> None: + """Configured cuRobo worlds survive config copies without cloning handles.""" + robot = _Robot() + cube = _RigidObject() + simulation = _Simulation(robot, {"cube_native": cube}) + registration = SimulationTaskProgramRegistration( + SimulationSceneBinding( + registry_id="scene", + rigid_objects=( + SimulationRigidObjectBinding( + entity_id="cube", + simulation_uid="cube_native", + collision_role=SceneCollisionRole.STATIC, + ), + ), + ), + _profile_binding(), + ) + factory = SimulationTaskProgramFactory( + simulation, # type: ignore[arg-type] + robot, # type: ignore[arg-type] + registration, + step_dt=_STEP_DT, + planner_cfg=simulation_environment_module.CuroboPlannerCfg(robot_uid=robot.uid), + ) + + assert factory._planner_cfg is not None + factory._bind_curobo_scene(factory._planner_cfg) + copied = factory._planner_cfg.copy() + + assert copied.world.rigid_objects is not None + assert copied.world.rigid_objects["cube"] is cube + + def test_standard_registration_owns_and_freezes_live_runtime_assembly() -> None: """The standard path preserves exact ownership through live assembly.""" robot = _Robot() From b8aeba1ecfa67c845918f2d80dff1a390daaf832 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 16:26:29 +0000 Subject: [PATCH 3/3] fix(task-program): reject null planner subconfigs --- .../task_program/integrations/simulation/environment.py | 8 ++++---- .../gym/envs/task_program/test_configured_integration.py | 7 +++++++ 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 525261793..c7f227cbf 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -152,8 +152,8 @@ def _planner_cfg_from_config( ) if planner_type == "curobo": - world = options.get("world") - if world is not None: + if "world" in options: + world = options["world"] if not isinstance(world, Mapping): raise TypeError("planner_config.config.world must be a mapping.") world = deepcopy(dict(world)) @@ -168,8 +168,8 @@ def _planner_cfg_from_config( "runtime-owned; declare dynamic scene collision roles instead." ) options["world"] = CuroboWorldCfg(**world) - auto_gen = options.get("auto_gen") - if auto_gen is not None: + if "auto_gen" in options: + auto_gen = options["auto_gen"] if not isinstance(auto_gen, Mapping): raise TypeError("planner_config.config.auto_gen must be a mapping.") options["auto_gen"] = CuroboAutoGenCfg(**deepcopy(dict(auto_gen))) diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 26d21e757..6c6c632e2 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -283,6 +283,13 @@ def test_planner_policy_decoder_validates_backend_and_options() -> None: path="execution policy.planner", ) + for field_name in ("world", "auto_gen"): + with pytest.raises(TypeError, match="must be a mapping"): + _decode_planner_config( + {"type": "curobo", "config": {field_name: None}}, + path="execution policy.planner", + ) + @pytest.mark.parametrize("task_name", ("repeated_pick_place", "open_drawer")) def test_single_task_program_composes_with_ur5_and_franka(